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ABSTRACT 

Lightweight, slender manipulators offer faster 
response and/or greater workspace range for the same size 
actuators than traditional manipulators . Lightweight 
construction of manipulator links results in increased 
structural flexibility. The increased flexibility must be 
considered in the design of control systems to properly 
account for the dynamic flexible vibrations and static 
deflections. This thesis experimentally investigates real- 
time control of the flexible manipulator vibrations. 

Models intended for real-time control of distributed 
parameter systems such as flexible manipulators rely on 
modal approximation schemes . A linear model based on the 
application of Lagrangian dynamics to a rigid body mode and 
a series of separable flexible modes is examined with 
respect to model order .requirements, and modal candidate 
selection. 

Balanced realizations is applied to the linear 
flexible model to obtain an estimate of appropriate order 
for a selected model. 

Describing the flexible deflections as a linear 
combination of modes results in measurements of beam state, 
(position, strain etc.), which yield information about 
several modes. To realize the potential of linear systems 
theory, in particular to implement full state feedback. 



knowledge of each state must be available. Reconstruction of 
the time varying modal amplitudes form strain measurements 
is examined. Reduced order observers are utilized to obtain 
estimates of the modal velocities, from the reconstructed 
modal amplitudes. State estimation is also accomplished by 
implement ion of a Kalman Filter . 

State feedback control laws are implemented based upon 
linear quadratic regulator design. Specification of the 
closed loop poles in the regulator design process is 
obtained by inclusion of aprescribed degree of stability in 
the manipulator model. 



CHAPTER I 


INTRODUCTION 

This thesis presents the results of an investigation 
into the control of a flexible manipulator. The 
investigation focuses on real time control experiments 
examining dynamic models, and control strategies proposed by 
past researchers. Analytical work performed was primarily 
conducted to adapt the models and controllers to the 
experimental system, and resolve discrepancies between the 
experimental results, and expected results. 

Documentation is provided for the experimental 
hardware utilized in the experiment, as well as the software 
generated to implement the controllers. Some tutorial is 
given in the appendices to bridge the gap between the 
various topics involved in this work. 

1.1 Organization and Readers Guide 

The purpose of this section is to present the 
reasoning behind the layout of the thesis, and provide a 
short topical description of each section as an aid in 
finding material of interest to specific readers. 

The main body of the work focuses on presenting 
experimental observations, and comparison of these results 
to analytical predictions. The appendices contain long 
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derivations, programs , etc. which may be useful and 
informative, but are not central to the objectives of the 
thesis effort. 

The first chapter introduces the thesis topic, 
discusses the organization, and identifies pertinent 
literature. The chapter concludes by defining the specific 
problem considered for this investigation. 

The second chapter, "Experimental Apparatus", briefly 
identifies the major components of the experimental system, 
and provides graphic representation of their interplay. 
Detail information on the hardware components, and 
electrical connections are identified in Appendix E. 

The third chapter, "Verification of the Linear Dynamic 
Model" , roughly outlines the process for generating the 
dynamic model used in the controller design algorithms. 
Experimental observations conducted to evaluate the model 
are presented and compared to digital simulation. A detail 
account of the modelling process is contained in Appendix A. 

The fourth chapter discusses analytical estimation of 
required model order. The method of balanced realizations is 
introduced and applied to the model. This provides 
quantitative substance to the qualitative results of chapter 
3 . 

The fifth chapter, "Measurement and Reconstruction of 
Flexible Variables", discusses reconstruction of the 
flexible variables, and selection of locations for strain 
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measurement from which the variables are obtained. 

The sixth chapter, "Reduced Order Observers", reviews 
the concept of state observation, and the fundamentals of 
reduced order observers. Application of reduced order 
observers to the estimation of modal velocities is discussed 
including specification of the measurement update gain to 
obtain robust implementation. Pole placement of the observer 
poles is evaluated experimentally. 

Chapter seven, "Optimal Regulator", addresses the 
performance of a deterministic optimal regulator design for 
the flexible manipulator. Various degrees of stability are 
prescribed in the design process, and the resultant 
performance recorded. Models with with one and two modes are 
examined . 

Chapter eight, "Kalman Filter", discusses the 
experimental determination of measurement noise, and 
presents the closed loop performance based on the Kalman 
Filter estimates of the states. The amount of plant noise is 
varied to obtain a robust filter. 

Chapter nine discusses several additional results, and 
observations which had significant impact on the experiment, 
but do not fit in the other sections. 

Chapter ten summarizes the major contributions and 
results of this work, and identifies future work. 

Appendix A gives a detailed account of the dynamic 


model generation. 
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Appendix B derives the frequency determinant for a 
Bernoulli-Euler beam. 

Appendix C generates the necessary equations, and 
solution technique for the Optimal Quadratic Regulator. 

Appendix D embraces the Kalman filter origins. 

Appendix E gives manufacturers data, results from 
tests conducted to verify component performance, and 

electrical connection diagrams. 

Appendix F documents the software routines generated 
for the micro-processor controller. 

1.2 Background 

A large body of analytical research applicable to the 
modeling and control of flexible manipulators has 

accumulated over the last two decades. The following 

paragraphs briefly chronicle the most pertinent efforts, and 
highlight major contributions. 

All physical systems can be described as continuous in 
space, and/or time, however, many systems are adequately 
described using simpler lumped models. To provide 
responsive, accurate control of flexible manipulators the 
distributed nature of the mass needs be included in the 
dynamic model. 

The dynamics of distributed systems has been under 
investigation for many years, and F.T. Brown [II], 1964, 
classified the general character of these systems, and at 
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the same time drew physical analogies between the 
distributed parameter systems of several fields. Shortly 
afterwards, this material started appearing in aerospace 
literature with respect to stabilizing flexible vehicles. 
For example, work by R.E. Andeen [12], 1964, applied these 
ideas to the stabilization of rockets. 

Modal control of a distributed parameter systems was 
discussed as early as 1966 by Lasso [13], and again by Lasso 
and Gould [14], 1966. Lasso and Gould described control law 
development using classical techniques, and discussed 
determination of modal quantities from measurements. The 
modal analysis technique, discussed by Lasso and Gould, is 
similar in concept to the reconstruction concept used for 
this study. Wykes, and Mori [15], 1966, reported on the 
applicability of this approach to the control of flexible 
modes in aircraft. 

Vaughan [16], 1968, applied wave propagation concepts 
to the control of bending vibrations. Vaughan was interested 
in determining impedance matches for a passive endpoint 
attachment. Vaughan obtained transfer matrices to describe 
the dynamic character of a flexible beam. The transfer 
matrices were similar to those latter developed by Book [19] 
to describe the dynamics of flexible manipulators. 

Komkov [17] discussed optimal control of a transverse 
beam oscillations in 1968. Mirro [18], 1972, was perhaps 
the first to discuss the feedback control of a flexible 
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manipulator. Mirro examined the usefulness of optimal 
regulators as applied to this problem. This was followed by 
Book [19], 1974, who examined the usefulness of transfer 
matrices in the modeling and control of flexible 
manipulators. In addition. Book drew conclusions about the 
response of flexible manipulators to feedback controllers, 
and discussed design of the flexible member. Neto [110], 
1974, examined the application of modal analysis and 
Lagrangian formulation of the dynamic system to the analysis 
and control of flexible manipulators. 

Schaechter [111], 1974, examined the control of 
flexible vibrations aimed at large scale space structures, 
and included some experimental investigations. The apparatus 
was a free hanging truss structure, with actuation suitable 
for . space applications. The paper served to document the 
apparatus, associated hardware, and modeling difficulties. 

Martin [112], 1978, analytically investigated the 
control of flexible mechanical systems, and specifically 
considered the performance of both collocated and non- 
collocated controllers. This work was aimed at large space 
structures, and paralleled the work of both Mirro, and Book. 
Balas [113], 1978, discussed modal control of large space 
structures and focused on the impact of controller designs 
and implementations based on truncated models. The control 
concepts intended for flexible space structure control 
usually consider multiple actuators distributed over the 
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lengths of open truss structures to control the undesired 
vibrations. 

Book, Majette, and Ma [114], 1979, continued to 
develop frequency domain techniques for the control and 
analysis of serial manipulators with multiple flexible 
links. Hughes [115], 1979, developed complete, general 
dynamic relationships for flexible chain systems applicable 
to flexible manipulators. Balas [116], 1980, continued to 
contribute to the field of space structure control with an 
investigation of modeling based on finite element 
techniques. The model, although suitable for small amplitude 
vibrations about an equilibrium configuration, did not 
properly account for rapid gross motions typical of 
manipulator applications . 

Fujii [117], 1980, developed functional observers for 
distributed parameter systems in recognition of the 
computational requirements of state observers. Truckenbrodt 
[118], 1981, concentrated on modelling a fixed base flexible 
manipulator that was similar to Neto's earlier work. 
Truckenbrodt, additionally compared the performance of the 
model to a simple test' device and found reasonable agreement 
for the frequencies of the first four vibratory modes. 

Balas [119], 1982, discussed a need for more practical 
control concepts satisfying constraints which arise in 
implementation. Sunada, and Dubowsky [120], 1982, developed 
finite elements for flexible manipulators which incorporated 
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the dynamic effects of gross motions encountered in robotic 
applications . 

Cannon and Schmitz [121], 1983, discussed 
characteristics of a very flexible manipulator with open 
truss construction, and in further work [122], 1985, 
investigated end-point position control. The end-point 
position was measured by an optical sensor external to the 
manipulator system. Zalucky and Hardt [123], 1984, examine 
compensation for static deflections occuring in flexible 
members using an optical sensor to measure deflections. 

Turic and Midha [124], 1984, obtain generalized 
equations of motion for elastic systems using finite 
elements. Turic, Midha, and Bosnik [125], experimentally 
examine the ability of the finite element technique of the 
previous reference to predict the motion of a four bar 
linkage. Weeks, [126], presents solution of the boundary 
value problem describing control of flexible structures by 
use of integral transforms. Weeks recognized that for real- 
time implementation the Greens functions could be 
approximated by expansions of selected eigenfunctions. Bodde 
n and Junkins [127], 1984, discussed optimization of the 
eigenvalues for structural controllers. Junkins [128], 1985, 
continued to examine the flexible structure eigenvalue 
placement problem using optimal linear quadratic regulator 
design. 

Meirovitch and Baruh [129], 1984, consider the 
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application of modal filters to reduce model order 
requirements in control of flexible structures. 
Sangveraphunsiri [130], 1984, applied optimal control 
methods to obtain controller designs for a single link 
flexible manipulator. Deterministic and stochastic steady- 


state regulators. 

as 

well 

as 

time optimal 

bang-bang 

controllers were 

simulated 

with 

linear, and 

nonlinear 

models. Major and 

Shain 

[131], 

1984, 

consider the 

control of 


a flexible truss suitable for space applications, and go on 
to discuss experiments on a single truss section in 1985 
[132]. 

Book [133], 1984, presented the "Bracing Strategy" to 
achieve higher effective stiffness for flexible manipulators 
in performance of certain robotic tasks. Lane and Dickerson 
[134], 1984, considered the application of visco-elastic 
damping materials to achieve passive control of flexible 
vibrations. Alberts et. al., [135], experimentally evaluated 
passive damping treatments applied to a single link flexible 
manipulator. Major and Maples [136], 1985, considered force 
control of manipulators with flexible structural elements. 
Naganathan and Soni [137], 1986, examine non-linear 
kinematic formulations of flexible manipulators using finite 
element techniques. 


1 . 3 Problem Statement 


The following section defines the specific physical 
configuration, and problem addressed in this investigation. 
A flexible arm limited to motions in the horizontal plane 
via rotation about a fixed joint is depicted in figure 1-1. 
The arm is constructed from a single continuous beam. The 
only control actuation available to the system is a torque 
delivered at the single rotational joint. The following 
measurements of the state of the system are available: 

1. Joint rotation angle 

2. Joint rotational velocity 

3 . Strain at least two locations 

A control torque u, is determined that will rotate the 
flexible beam from one arbitrary orientation to another 
specifiable orientation quickly and accurately. This 
function utilizes the measurements to provide information on 
the state of the system. 

Since the proposed investigation relies heavily on 
experimental investigation, the performance of physical 
components add complexity to the problem. Linearity, and 
responsiveness of the torque source, coulomb and viscous 
friction of the joint, and measurement noise are but a few 
of the factors impacting the problem. 


1 . 4 Contribution 


The major contributions of this thesis lies in the 
evaluation of truncated models for use in the control of 
flexible manipulators. Chapter 3 identifies the importance 
of mode selection and model order. Chapter 4 proposes a 
method for apriori estimation of the required model order. 
Chapter 5 presents a measurement scheme for the time varying 
modal variables. Chapter 6 develops a reduced order observer 
for modal velocities. Chapters 7, and 8 evaluate the 
performance of controllers based on these models. Chapter 9 
contains additional information gleaned from the series of 
experiments. All this information forms the foundation for 
further work in controlling distributed parameter systems. 



Figure 1-1. Single Link Manipulator 


CHAPTER II 


EXPERIMENTAL APPARATUS 

This section of the thesis introduces the experimental 
manipulator system. The graphic configuration of the major 
subsystems and their major functions are identified in this 
section. This is done to establish a point of reference for 
the following chapters, and to set the physical scale of the 
experiment. Manufacturers, specifications, experimental 
measurements made to assure the performance parameters of 
key components, and detail diagrams identifying the actual 
electrical connections are identified in Appendix E. 

The experimental hardware is separated into three 
major subsystems: 

• Micro-Processor System 

• Manipulator 

• Signal Conditioning 

Figure 2-1 graphically depicts the system hardware. 
The micro-processor subsystem consists of a high speed 
sixteen bit micro-processor with floating point hardware, 

mass storage, and sensor I/O for data acquisition and 
control. 

The manipulator subsystem consists of the flexible 
link, torque motor, payload, and sensors. The link sets the 
scale for the experiment. The arm is a four foot aluminum 


beam oriented so that axis of increased flexibility is in 
the horizontal plane. Four feet is longer than lengths of 
many "pick and place" industrial robots, yet significantly 
shorter than proposed scales for flexible manipulators [III- 
2]. A commutated DC torque motor provides the motive power 
for the link. 

The payload is provisioned for the addition of weights 
to allow investigation of the systems sensitivity to payload 
mass. Several sensors are attached to the flexible 
manipulator in order to provide information about the state 
of the link. A potentiometer is mounted to the top of the 
actuator shaft to measure the rotation of the joint. A 
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Figure 2-1. Graphic Configuration of 
the Experimental Apparatus 




tachometer located under the motor housing is used to 
measure the angular velocity of the joint. Strain gages, 
mounted at the base and midpoint of the link, provide 
measurements of the beam deflections. 

The signal conditioning subsystem provides interface 
electronics for the strain gages, angle sensor, tachometer, 
and torque motor. The strain gages require bridge circuitry 
and amplification. The angle sensor and tachometer require 
buffer amplifiers to isolate them from the line loads and 
provide analog scaling. Four band pass filters are also 
provided. The torque motor is driven by a large DC servo- 
amplifier configured to provide currents proportional to the 
input voltage. 
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CHAPTER III 

VERIFICATION OF THE LINEAR DYNAMIC MODEL 

The material in this section of the thesis describes 
the linear model which formed the basis for most of this 
investigation into the control of flexible manipulators. 
The initial sections discuss the modeling process, and steps 
taken to verify system parameter^ and algorithm 
implementation. The latter section compares simulations of 
the model to experimental measurements. 

3.1 Model Generation 

This sub-section describes the formation of a linear 
state space model for the flexible manipulator. The process 
for forming the model will be outlined in this section; a 
detailed description is contained in appendix A. The first 
step of the process is to describe the position of every 
point -along the flexible manipulator. A linear combination 
of vibratory modes to describe flexible deflections, and a 
rigid body motion of the center of mass is selected. A 
manipulator with a rigid body rotation and flexible 
"clamped-mass" mode is depicted in figure 3-1. 

The flexible deflections are described by an infinite 
series of separable modes. Separability in this instance 
refers to describing the flexible deflections as a series of 



assumed modes [nil] which are products of two functions, 
each of which is a function of a single variable: one a 
function of a spatial variable, and the other a function of 
time. This is noted as: 

w(x,t)= ^ i (x)g i ( t) , for i=l,2 — n (3.1) 

This separability is important in later phases when the 
model is formed in terms of time varying variables only. 

Next the kinetic and potential energies are derived. 
The distributed character of the flexible manipulator is 
taken into account via integral expressions over the mass of 
the entire system in forming the energy expressions. The 
integral for calculating the kinetic energy, KE, has the 



Figure 3-1. Flexible Manipulator 
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following form; 


KE = | 


R*Rdm 


(3.2) 


where R, the absolute velocity vector, and mass, range over 
the entire system. The potential energy, PE, of the system 
is stored in the flexible modes and can be attributed to 
"modal stiffnesses", K^, which are evaluated by integrals 
over the length as shown in equation A. 12. Lagrange's 
equations of motion can be formed from the energies; 


d 

dt 


3KE 


3 ^ 


3 PE 
Hi 


= Qi 


(3.3) 


where the are the coordinates, and Q i are the generalized 
work terms associated with each coordinate. Turning the 
computational crank on the various differentials and 
integrals as carried out in appendix A results in a coupled 
set of second order dynamic equations with familiar form,* 


[M] z + [K]z = [Q] 

Z = [8,q 1 (t) ,q 2 (t) ,q n (t)3 


(3.4) 

(3.5) 


M is a mass matrix, K represents stiffness, and Q the input. 
The dynamic equations are easily organized into a state- 
space model as shown in equation (3.6). The motor torque at 
the joint, and the generalized work terms, Q^, are then 
related to the rotation of the joint with each variable. 
Examination of the form of the model reveals the expected 
result that the coupling between the modes, and the rigid 
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body motion occurs from inertial terms of the mass matrix. 

Equation (3.6) depicts a 2(n+l) order linear model 
where n is the number of included modes. Non-linear terms 
arise from the evaluation of equation (3.2) for the kinetic 
energy, and the specific assumptions employed to obtain a 
model containing only linear terms is discussed in Appendix 
A. 



The remaining task in generating a trial model is the 
selection of the flexible modes to be used in forming the 
constant mass and stiffness matrices. 

The path chosen in this work is to select admissible 
functions as candidates which are solutions to closely 
related problems. These solutions are eigenfunctions for 
selected "clamped-mass" , and "pinned-mass" boundary value 
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problems . Clamped describes a boundary condition where the 
joint is fixed against rotation, pinned describes a joint 
with motor inertia free to rotate, and mass describes the 
condition of the payload at the other beam boundary. The 
admissible functions will then satisfy the differential 
equation, the essential or geometric boundary conditions, 
and the natural boundary conditions of the free vibration 
problem. 

Appendix B describes the development of the 
differential equation for a Bernoulli-Euler beam and the 
solution of selected boundary value problems. The problem is 
formulated in terms of a frequency determinant for the 
determination of the eigenfunctions and the associated 
frequencies . 

The experimental apparatus introduced in Chapter 2 was 
used to examine the model's performance. Table 3-1 
identifies the important parameters of the beam which were 
used as inputs to the modeling process. 


Table 3-1. System Parameters 


Flexible Beam - 

Material 

Form 

Length 

Moment of Inertia 
El Product 


- Aluminum 6061-T6 

- Rectangular 3/4 x 3/16in 

- 48 in 

- 4.12E-4 in 2 

- 4120 lbf-in 4 


3.3 Parameter and Program Verification 
This section describes experiments conducted to verify 
system parameters and program implementation of the model 
generation process. Initially the frequencies determined via 
the Bernoulli-Euler beam equations with clamped-mass , and 
pinned-mass boundary conditions are compared to measured 
eigenvalues of the beam. This examines beam length, modulus, 
and density parameters, as well as the suitability of the 
chosen boundary conditions. 

Figure 3-2 shows a measured transfer function from 
random torques input by the motor to strain at the base of 
the beam. The peaks correspond to "clamped-mass" modes, as 



Figure 3-2. Frequency Response 
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the clamped boundary condition results in modes having 
maximum moments at the base of the beam. The valleys are 
associated with "pinned-mass" modes, as this boundary 
condition results in modes which have small moments which 
rotate the motor inertia. Martin [III2] discusses 
measurement zeros which occur in flexible structures. 

The vibratory modes were additionally calculated by 
the frequency determinant described in Appendix B. Table 3-2 
compares the measured modal frequencies to those computed 
using the Bernoulli-Euler Beam. The application of the 
Bernoulli-Euler formulation to the "clamped mass" case 
agrees very well with the measured frequencies, however, the 
"pinned-mass" conditions were not as accurate. 

The poorer agreement for the pinned case is attributed 
to the friction found in the joint hardware; this is a 
difficult condition to model and may have a significant 
effect for the small amplitude motions used during the 
tests . 


Table 3-2. Comparison of Modal Frequencies (Hz) 



Clamped 

-Mass 

Pinned- 

■Mass 

Mode 

Measured 

Calculated 

Measured 

Calculated 

1 

2.08 

2.096 

10.01 

9.732 

2 

13.92 

13.989 

33.45 

31.608 

3 

41.38 

41.524 

70.56 

62.683 

4 

81.18 

81.225 


148.768 

5 


136.352 


216.048 


The next step checked the model generation algorithm. 
Normalization of the modal masses allows the checking of the 
computations by examining the diagonal components of the 
stiffness matrix. The stiffnesses should be the square of 
the modal frequencies input to the process. 

The algorithm was checked for both the clamped-mass 
modes and the pinned-mass modes. Table 3-3 presents a 
comparison of the modal frequencies input to the modelling 
process. The results are very good, however it was 
necessary to use higher precision computations for the 
higher modes. 


Table 3-3. Comparison of Frequencies Determined 
by Stiffness Computations 


Clamped-mass 

1/2 

Input Stiffness ' 

2.096 Hz 2.096 
13.989 13.989 

40.552 40.524 

81.225 81.225 

136.352 136.344 


Pmned-Mass 


Input 
9.732 Hz 
31.608 
62.683 
148.768 
216.048 


Stiffness 1 ^ 

9.732 

31.608 

62.683 

148.768 

214.621 


3 . 4 Dynamic Response Comparison 
The previous section provides confidence that the beam 
parameters have been properly identified and modeled by the 
Bernoulli-Euler beam. The computational procedure has 


additionally been checked. The major questions concerning 
the model can now be investigated: 

• Choosing the Modal Candidates 

• Required Model Order 

• Is a Linear Model of the Coupling Adequate 

The following paragraphs describe simulations and 
experiments conducted to gain insight into the answer to 
these questions. The simplest and best understood 'controller 
for flexible arms is a collocated controller, that is, a 
control system where the measurement and actuation is 
located at the same point. A collocated controller was 
implemented for the experimental system which applied a 
position gain to joint angle measurements, and a rate gain 
to angular velocity measurements. 

The position gain was selected to provide the rigid 
body mode with a characteristic time of one second. The rate 
gain was selected to provide a damping ratio of 0.7. Higher 
gains could be selected which stress the impact of 
flexibility on the control strategy, however the chosen 
gains -provide a good starting point well within the 
operating parameters of the system. 

Figure 3-3 displays the measured response of the 
experimental system to a step change in desired joint angle. 
Strain measurements presented in the figure, while not used 
in the controller, provide an indication of the relative 
modal amplitudes. 


24 






The dynamic model was discretized, and simulated for 
the step angle change. Small amounts of damping, (typically 
damping ratios ranging from .007-. 010, based on transfer 
function measurements using an impulse hammer as the input 
and strain at the base as the output [ III 3 ] ) were introduced 
into the model for the flexible modes. Additionally, 
hysteretic joint friction was modeled as coulomb friction 
[III4] and included in the digital simulation. Inclusion of 
modal damping and hysteresis in the simulations improved the 
agreement of the models especially in the time interval 
after the large initial transients had occurred. 

Figure 3-4 shows the results for a model implemented 
with five clamped-mass modes, while figure 3-5 presents a 
model using two clamped-mass . modes. The last case simulated 
used five pinned-mass modes as inputs to the modeling 
process. This is presented in figure 3-6. 

The simulations based upon clamped-mass modes agree 
the best with measured responses. Surprisingly the model 
implemented with only two clamped-mass modes agrees almost 
as well if not better than the higher order model. The 
poorer agreement of the higher order model is probably due 
to poor estimation of the damping by use of the impulse 
hammer measurements. Additional damping in the higher modes 
from the joint is likely when the joint is in motion. Should 
the actual manipulator exhibit the higher amplitude 















vibrations in the higher frequencies, as shown in figure 3- 
4, larger models including the higher flexible modes would 
be required. This presentation maybe somewhat misleading as 
better determination of damping for the higher modes could 
provide better results. It is apparent that a dominant 
portion of the response is adequately characterized by as 
few as two modes. 


3 . 5 Summary 

A modelling process to generate a linear model for use 
in controlling flexible manipulators was presented, and 
compared to experimental measurements for a position, and 
rate feedback controller. The model agreed favorably with 
the measured response for a selection of clamped-mass 
assumed modes. The dominant parts of the transient response 
were characterized by inclusion of as few as two assumed 
flexible modes. 

The selection of appropriate assumed modes must 
consider the feedback law, as the applied torque dominates 
the boundary condition at the base of the beam. Clamped-mass 
modes yielded good results for the simple collocated 
controller, however this may not prove true for more 
sophisticated controllers. 


CHAPTER IV 


MODEL ORDER REDUCTION 

Truncation of the modal series is required to achieve 
a model suitable for implementation in real-time. Based on 
initial estimates of computational speed a model treating 
two flexible modes was targeted for this study. While 
verifying the linearized model, as discussed in the earlier 
chapter, the two mode model was found to characterize the 
major portion of the response. This process was completely 
arbitrary, and satisfactory performance of the controller 
was in no way guaranteed. It was therefore desirable to seek 
an analytical means to estimate the required model order, 
especially when looking forward to research on multi-link 
systems . 

Professor Dorsey, Electrical Engineering Dept./ 
Georgia Tech. , suggested the use of balanced realizations as 
a method for providing the model order estimate. Balanced 
realizations is based upon singular value decomposition of 
linear systems [IV1]. This suggestion was based upon the 
similarity of the model used in this work to one he was 
familiar with in power systems research. The linearized 
version of a power system model is shown in figure 4-1. The 
states are the A 6 variation in angles, and Au> variation in 
speeds for the generators. M represents the inertial matrix 
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of the generators, y represents damping. T represents the 
change in electrical power with respect to the angle 
variations A<5, and L depicts the change in electrical power 
with respect to the load power. The linearized model for the 
manipulator is repeated in figure 4-2. The structural 
similarities are readily 




Figure 4-1. Power System Model. 
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apparent between the two types of systems. Additionally, Dr. 
Dorsey had robust software machinery evaluating the power 
system models. Troullinos, and Dorsey [IV2] wrote an 
introductory paper on model order estimation with this 
method, and that material is presented next to familiarize 
readers with the method before discussing its application to 
the flexible manipulator. 



Figure 4-2. Manipulator Model. 



where T is an n x m matrix, an intuitive, geometric 
interpretation of the potential redundancy of the system's 
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variables can be achieved using spaces R(T), R(T' ) and their 
respective orthogonal complements Ker(T'), and Ker(T). 
Ker(T) is the nullspace of T, the subspace spanned by all 
vectors x such that Tx=0. T' is the transpose of T. Figure 
4-3 summarizes the relationships between these four 
subspaces . 

Assume that m<n. Any m-dimensional vector x can be 
represented as the sum of its projection x r , on R(T') and 
x^, on Ker(T). That is x = x r + x^. Similarly any n- 
dimensional vector y can be represented as y= y r +y k , the sum 
of its projections on R(T) and Ker(T*). Then y r = Tx = T(x r 
+ V = Tx r + 0 = T V Thus ' effectively, T can be thought 
of as an invertible transformation from R(T’ ) to R(T). The 
inverse transformation that takes Tx back to x is called the 
psuedo-inverse [ IV3 ] . 



Figure 4-3. Relationship Between Subspaces. 
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The subspace decomposition described above provides an 
elegant and intuitive picture of linear transformations. The 
necessary complement to this theoretical picture is a robust 
computational method of decomposing the matrix T. Every n x 
m matrix T can be factored as T = UZV' with U an n x n 
orthogonal matrix, V an m x m orthogonal matrix and I an n x 
m matrix that has the special form 


( 4 . 2 ) 


where the are the nonnegative square roots of the 
eigenvalue of the matrix T'T. If T is of maximum rank, then 
all the a's will be non-zero. If the rank of T is r<m, then 
only the first r diagonal entries will be nonzero, in which 
case T can be written as 


r 0 

T = [U U, ] 

r * 0 0 


( 4 . 3 ) 


where l r = diag( • • • ,a r ) and u r ' u k' V r ' and v k are 
orthogonal matrices which span R(T), Ker(T’), R(T) , and 
Ker(T), respectively. 

The singular values have the crucial property that if 
T is perturbed slightly to T^ = T + AT, then the singular 
values of T^ will be "close" to the singular values of T. 
Thus if T is of r<m then the decomposition of the 
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transformation would yield 


t a = [u rA u k 4 ' 


z rA 0 


‘kA- 



1 

< 

■ M • 
> 

1 

J 

L V kA-l 


(4.4) 


where the singular values of would be small compared to 

the singular values of I Thus T = u rA s rA V rA w ould be a 

good approximation of the original transformation T = 

U^V’. Even in the case where T is known exactly, if the 

singular values can be segregated into two groups where the 

members of one group are much smaller than the members of 

the other, then the matrix u .1 .V' . may still be a useful 

rA rA rA x 

approximation to the matrix U r £ r V r* It is this latter point 
of view that is exploited in the next section. 


4.2 Singular Value Decomposition Applied to Dynamic Systems 
Given the linear time invariant system 

x(t) = Ax(t) + Eu(t) (4.5) 

y(t) = Cx( t ) (4.6) 

two transformations are of interest. One is the 
transformation T c (t^) from an initial state, x(t Q ), to a 
subsequent state, x(t^), under the influence of an input 
u(t). The other is the transformation Tg(t^) from an initial 
state x(tg) to a subsequent output y(t^), with no input. The 
first transformation provides information about the 
controllability of the system. 


while the 


second 
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information about the observability of the system. If the 
system has uncontrollable or unobservable states then the 
state space can in a sense be interpreted as degenerate, or 
not of full rank. 

The linearized model for the flexible manipulator is 
assumed to be controllable and observable; this was checked 
in the computations. What is of interest are states that are 
nearly uncontrollable or unobservable. As in the static case 
these estimates can be obtained using singular value 
analysis for the system [IV4]. With t Q = 0, the 
transformation from a given initial condition x(t Q ) = x Q , 
with input u(t) to x(t 1 ) is described by [IV5]; 

rt-. 


x(t 1 ) = e At l x 0 + 


e A(t l" T) Bu( t ) dx 


(4.7) 


For the special case where x ( t Q ) = 0, and u(t) = 6(t)u, 

where u is a vector of impulse magnitudes. 

At. 


x(t 1 ) = e 


Bu 


(4.8) 


It can be shown [IV6] that R[T (t.)] = R[W (t 1 )] where 

rt., 


W o (t l» = 


At A * t 
e AT BB'e A T u( t ) dx 


(4.9) 


Similarily for the unforced case 


At. 


y(t x ) = Ce x Q 


(4.10) 



Thus Ker[T Q (t 1 )] = Ker[W Q (t 1 )] where 




w 0 (t l> = 


i A ’ T C’Ce AT di 


(4.11) 


w c (ti) and Wgft^) exhibit the same robustness found in the 
static case [IV7]. Thus if a singular value decomposition of 
W c (ti) yields 


W 


t“l u 2> 



(4.12) 


where the singular values of are much larger than the 
singular values of I 2 , then the number of nearly 
uncontrollable states is the dimension of £ 2 . A similar 
argument holds for W Q ( t 1 ) . 

One more step, that of balancing the system, is 
required. The size of the singular values can be interpreted 
as the "strength" of the individual states. That is, the 
singular values measure the extent to which a state is 
influenced by inputs and initial conditions. It can happen 
that a particular state will appear strong from the 
controllability perspective and weak from the observability 
perspective. A given state may, for instance, be strongly 
amplified by the B matrix and equally attenuated by the C 
matrix, or vice versa. Balancing the system is a method of 
equalizing the controllability and observability "ratings" 
of a state. That is, assuming that the system (eqns 4.5-6) 
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is minimal, balancing is achieved by finding a 
transformation, x(t) = Tz(t) such that for the system 


z( t) = Az( t ) + Bu( t) 


(4.13 


y(t) = Cz ( t ) 


(4.14) 


the controllability grammian W (t^) and the observability 
grammian ftg(t^) are identical. Noting that the 
controllability and observability grammians of equations 


(4.5-6) and (4.13-14) are related by 

W c (t 1 = T" 1 W^(t 1 ) (T* 1 ) ' 


(4.15) 


W^(t 1 ) = T' wj(t x ) T' 


(4.16) 


the desired transformation T can be obtained as follows. Let 


w c (t l> ■ “c (t l> I c (t l ,u i< t l> 


(4.17) 


W 0 ( V = U 0 (t l ,J 0 (t l ,O 0 (t l ) 


(4.18) 


be singular value decompositions of the two grammians of the 
unbalanced system. Define 


Hit I = wwww 


(4.19) 


with singular value decomposition 


H(t 1 ) = U H (t 1 )I R (t 1 )V^(t ) 


(4.20) 


and choose 




(4.21) 
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It is then a straightforward calculation to show that 

1 

_1 

C u c (t i )!: c (t i )u i (t i ) nu c (t 1 )!:; 1 (t 1 )v H (t 1 )x|(t 1 )] 


(4.22) 


= s H ( tl ) = w; (tl ) 


By rank ordering the singular values of H(t^), i.e. the 
diagonal elements of I H (t^), the balanced model can be 
divided into two subsystems, one associated with the larger 
singular values and one associated with smaller singular 
values as 


*ll 


A 11 A 12 

- A 21 A 22 


(4.23) 


y = [c, c 2 ] [ *i] 


(4.24) 


The reduced order model is the subsystem associated with the 
large singular values: 

z^ = z + B^u (4.25) 


Y= C x Zl 


(4.26) 
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4,3 Application to Flexible Manipulator Model 
Using modeling software developed for this thesis a 
model was generated including one rigid mode, and nine 
flexible modes. A small joint angle position gain, 50, and 
velocity gain, -10, was introduced with small amounts of 
modal damping, (damping ratios of .007-. 010), to insure that 
all eigenvalues had negative real parts before executing the 
analysis software. 

To evaluate the contribution of each mode, and thus 
infer the required model order, requires some measure of 
performance. The selected performance index is that proposed 
by Moore[IV8]. For the manipulator model with N/2 = 10 
modes, one rigid and nine flexible, the singular values are 
rank ordered from the smallest to the largest. Then a 
performance index of the form: 



p(i)= (4.27) 

2N-2 1 

k=2i+l 

is evaluated for i = 1,2,..., N-2 since the singular values 

occur in complex conjugate pairs. The numerator and 
denominator of p ( i ) represent, respectively, the square root 
of the sum of the squares of the singular values associated 
with the aggregated and unaggregated modes. Each mode 
aggregation reduces the number of singular values by two. 


and hence at an aggregation level of (N-4)/2, the entire 
manipulator model has been reduced to one rigid and one 
flexible mode. 

The linear model for the flexible manipulator was 
evaluated with the above process by Dr. Dorsey with software 
he had created for analysis of power systems. The result of 
the computations are presented graphically in figure 4-4, 
and agree well with the qualitative results presented in 
chapter3. As can be seen from figure 4-4, the curve makes a 
sharp change in slope after an aggregation level of six, and 
the performance level quickly rises. This indicates that 
most of the model is represented by three modes, one rigid 
and two flexible, at an aggregation level of (N-6)/2 or 7. 



Figure 4-4. Aggregation Level 



CHAPTER V 


MEASUREMENT AND RECONSTRUCTION OF FLEXIBLE VARIABLES 

This section describes the approach used to determine 
the state variables associated with flexible deflections. 
The position of any single point along the beam can be 
described as a combination of the rigid body motion, and 
flexible deflections. The approach taken in this work is to 
describe the flexible deflections by a modal series. This 
means that the flexible deflection is a linear combination 
of variables, and any single measurement of beam state, 
position, or strain etc., yields information about several 
modes . 

To realize the potential of linear systems theory, in 
particular to implement full state feedback, knowledge of 
each state must be available to the control law. The task 
then is to gain information about each flexible state 
variable from the measurements. 

5.1 Measurement and Reconstruction 

Joint angles, and joint rotational speeds can be 
measured directly as for rigid manipulators, however for 
state feedback control of manipulator flexibility it is 
desirable to make direct measurements of the modal 
variables. Three types of measurement are currently 



receiving attention for experimentally controlling 
flexibility in manipulators: optical measurement of end 

point position [Vl-2], optical measurement of deflection 
[ V3 ] , and measurement of strain on the link [V4]. The 
measurement selected for this work is strain. Strain 

measurement has the following beneficial aspects: 

• Isolates beam variables from rigid motions. 

• No restrictions on work envelope or 
positioning. 

• ' High compatibility with harsh industrial 

environments subjecting the sensors to process 
sprays of oils, solvents, and dispersed solids. 

• Low cost sensor and driving electronics with 
simple technology base. 

Additionally, the concept presented here can be 

applied almost directly to optical measurements of 
deflection. Measurement zeroes observed in end-point 

position measurements [VI] by sensors mounted external to 
the manipulator may adversely affect application of 
reconstruction to this means of measurement. 


5.2 Strain Relationships 

The moment at any location along the beam is related 
to the curvature of the beam: 


M = El d w(x, t ) 
9x 2 


(5.1) 


The stress of the fibers along the surface of the beam due 
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to bending can be determined from the moment: 


a = Me 
I 


(5.2) 


The strain due to bending is then: 


e = a 
E 


(5.3) 


The strain can now be given in terms of the beam deflection 
w(x, t) : 


e ( x , t ) = c 3 w(x,t) 

3x 2 


(5.4) 


Assumed mode representation of the flexible deflections can 


be expressed by: 


w(x,t) = ( x) q ± ( t) 


(5.5) 


The strain can then be represented in terms of the assumed 
modes as : 


£(x,t) = cZg. (t) d (x) 
x 2 X 

dx 


(5.6) 


Equation (5.6) can be expanded to clearly show the 
contributions of each flexible mode to the measurement of 
strain at a location x = a on the beam. 


e(a,t) = c[ d 2 d) 1 (a)g 1 ( t) + d 2 <fr 2 ( a)g 2 ( t ) + 
dx 2 dx 2 


(5.7) 


0-n (a >*n (t)] 
dx z 


For strain measurements at several locations a, b, . . . ,m 
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this relationship can be presented in matrix form: 


e(a,t) 


cd z <f) 1 (a) 

dx 2 

cd^<f) 2 (a) 

dx^ 

• • • cdd> ( a ) 

2 n 

dx z 



e(b,t) 

— 

A (b) 

2 X 

dx z 

cd 2 <j>2 (b) 
dx 2 

• • • cd 2 cj) _ ( b) 

2 n 

dx z 


q 2 (C) 

• 

• 

• 


• 

■ 

• 

• 

• 

• 

• 

• 

• 


q 3 (t) 

e(m,t) 


2 

cd <ti (m) 
2 1 
dx z 

2 

cd 0 ( m ) 

2^ 

dx" 1 

2 

• • • cd d) (m) 

2 n 

dx z 


• 

• 

• 

9 n (t) 


(5.8) 

The relationship depicted above relates the flexible 
variables to the strain measurements, and can be expressed 
as a variable transformation T _1 . 

e = T _1 q (5.9) 

The desired form of the transformation is to "reconstruct" 
the flexible mode amplitudes from the strain measurements. 

q = Te (5.10) 

The number of strain measurements m, is practically a small 
number, (2, 4 etc.), while the number of modes, n, typically 
used to characterize vibration is large, (10, 20 etc.). This 
would result in a rectangular matrix, and direct inversion 
to obtain the desired reconstruction matrix T would not be 
possible. In the case of more measurements than modes to be 
determined, a least squares weighting may be appropriate. 
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Based upon previous experimental results , discussed in 
chapter 3 , it was decided to investigate a model based upon 
two assumed modes with reconstruction accomplished from two 
strain measurements. This case results in a square matrix 
T~^. Sensor locations for this case can be selected which 
provide independent measurement of the two modes assuring 
that T exists. 


5 . 3 Sensor Placement 

The next problem to be addressed is the placement 
strain sensors. The ' specific type of sensor, 
arrangement to reduce sensitivity to temperature, 
transverse stresses is discussed in appendix E. 


of the two 
and bridge 
axial, and 



Figure 5-1. Moment Diagram. 
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The location of the sensors should be selected to 
provide measurement of significant signal levels, and yet 
enhance discrimination between the measured modes. Figure 5- 
1 presents moment diagrams for the first three clamped-mass 
modes . 

All the clamped mode shapes had a peak at the end of 
the beam attached to the joint. It was decided to place one 
of the two sensors at the base of the beam to insure 
measurement of the first mode. Further examination of the 
moment diagram shows that the second mode moment has a peak 
just past the midpoint of the beam. Additionally, note that 
the third mode moment has a zero close to the midpoint of 
the beam. 

It was decided to place the second set of strain gages 
at the midpoint of the beam. This provides good measurement 
of the second mode, yet avoids measurement of the third 
flexible mode. The sign change in the second mode moment, as 
opposed to the first moment should help the reconstruction 
algorithm to discriminate between the modal measurements. 



CHAPTER VI 


SEDUCED ORDER OBSERVERS 

The main thrust of this section is to describe the 
application of reduced order observers to the control of a 
flexible robot arm. Specifically, estimation of the 
unmeasured modal velocities via the application of a 
Luenberger [VII] reduced order observer is presented. 

This section begins with a brief section on estimation 
of state variables with full order observers and the 
associated error dynamics followed by a discussion of the 
motivation for using a reduced order observers. 

The general relationships for reduced order observers 
is then developed including expressions for the error. The 
reduced order observer is then applied to the control of a 
flexible arm. Design freedom and robustness of the observers 
concludes this section. 

6 ♦ 1 Observation of the State of a Linear System 

Luenberger [VI 2] is recognized as pioneering much of 
the work on the observation of linear systems , however the 
following derivation is a compilation of the works by 
Luenberger, Gopinath[VI3 ] , and Fortmann[ VI4 ] . Equations 
(6.1-3) provide a state space description of a forced linear 
system with state feedback. 
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Ax + Bu 

(6.1) 

o 

X 

n 

n 

H 

(6.2) 

u = Ky 

(6.3) 


For such a system knowledge of the initial state x Q , 
and input history are sufficient for determination of the 
state at any later time. However, many times the measurement 
matrix C is not an identity matrix, and alternate schemes 
must be utilized to realize the desired state controls. 
Figure 6-1 depicts an open loop observer for estimating the 
states of a linear system. The subscript m designates 



Figure 6-1. Open Loop Observation 
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modeled elements, while the subscript p refers to elements 
of the actual physical plant. Unavailability of information 
regarding initial states and modeling errors make open loop 
observation of the state variables impractical for most 
control applications. Figure 6-2 graphically depicts an 
observer utilizing measurements of the plant to improve the 
observation and reduce the sensitivity of the estimates of 
the to modeling errors, and initial states. 

For the closed loop observer of figure 6-2 the 
estimated, or observed state z is described by; 



LC(x-z) + B m u 


(6.4) 


K T 
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and the plant state dynamics as; 

x = Ax - B u (6.5) 

6.1.1 Error Dynamics 

Subtracting equation (6.4) from (6.5) yields and 
expression for the time rate of change of the error; 

X-Z = Ax - A z - LC(x-z) + ( B -B ) u (6.6) 

pm pm 

Accurate modeling of the plant dynamics and input dependence 
results in a simpler form of the error expression; 

e = (A-LC)e (6.7) 

where; 

e = x - z (6.8) 

Examination of this expression for the error dynamics 
shows that a proper selection of the measurement gain matrix 
allows an arbitrary specification of the rate at which the 
error decays. Luenberger [VII] proved this to be true as 
long as the system was completely observable, i.e. the pair 
AC was observable. Additionally, the error dynamics 
described by equation (6.7) will dominate the behavior even 
when some modeling errors exist, as long as measurement gain 
L provides the major contribution to the negative real parts 
of the eigenvalues. 
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6.1.2 Separability 

The composite system can now be written in the 
following form; 



0 

A-LC 


- 

-x- 


r B i 



+ 


- 

-z- 


-B- 


u 


(6.9a) 


for systems without feedback, and for systems with feedback 
based upon the estimated states; 



-BK 

A-LC-BK 




(6.9b) 


The eigenvalues of the composite system can be examined more 
readily after a simple transformation T is applied to the 
description. 

T = [ j _° ] (6.10) 

Premultiplication by T and postmultiplication by T -1 
transforms the systems described by equations (6.9a) and 
(6.9b) into the following forms; 



( 6 . 11 ) 


For systems without feedback this system obviously has the 
eigenvalues of both the plant, and the observer error 
dynamics. For systems with feedback based upon the estimated 
states ; 


-x 

L z 


I 


I 


r A-BK -BK 



rB-| 

-B- 


0 


A-LC 


U 


( 6 . 12 ) 
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This system possess the closed loop poles of the plant in 
addition to the eigenvalues associated with the error. This 
separability allows for the design of the observer as a 
separate task from that of determining the state feedback. 


6 . 2 Luenberger Reduced Order Observers 
Direct measurement and reconstruction provides joint 
angle, joint velocity, and modal amplitude data for the 
controller. A reduced order observer can be designed to 
estimate the missing modal velocity amplitudes. The main 
advantage of a reduced order observer over full state 
estimation lies in computational savings; this translates 
into higher sampling frequencies during implementation. 


The 


following 


r>a r* a rrr a r*\V> o 

V* 4. MJMiAW 




w ui rw 


Luenberger [VII ] , and Gopinath[VI3 ] , reviewing the 
development of the equations which describe the behavior of 
reduced order observers. The system represented by (6.1-3) 
can can be partitioned into measured and unmeasured states 
as follows; 


X 1 

= 

A u 

A 12 


h 

+ 


*2 


A 21 

A 22 


X 2 


B 2 


C = |l 0 


(6.13) 

(6.14) 


x^ are the m measure states while x 2 are the unmeasured 
states to be estimated. Figure 6-3 presents a block diagram 
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of a reduced order observer which is described by; 

2 = A 22 z “ LC(X2~z) + + B 2 U (6.15) 

2 is the estimate of the states X 2 * The error dynamics for 
this system can be obtained by subtraction of equation 
(6.18) describing the unmeasured states from the estimation 
equation (6.15). 

= ^ A 22 " LA 12^ e 2 (6.16a) 

e 2 = x 2 “ 2 (6.16b) 

This estimate depends directly upon the measurement of the 


states it is 

desired 

to 

determine . 

The 

dependence 

on 

measurements of 

x 2 can 

be 

eliminated 

via 

substitution 

of 


(6.20) with the following result; 

2 = (A 2 2”LA 1 2)z + - LA^ 1 x 1 + ( B 2 ~LB 2 )u (6.17) 



Figure 6-3. Reduced Order Observer 










This result, although direct, allows little insight and may- 
cause some confusion. The following derivation follows a 
more heuristic path, and provides more insight into the 
derivation process. 

To accomplish this, first cull the expressions for the 
unmeasured states from equation (6.13); 

^2 = A 22 X 2 + A 21 X 1 + B 2 U (6.18) 

The quantity, 

A 21 X 1 + B 2 U (6.19) 

which appears in equation (6.15) can be considered as a 
known input as it contains only measure and computed 
quantities. Next, the expressions for the measured state 
velocities can be separated and reorganized as; 

X 1 ~ A ll x l “ B l u = A 12 x 2 (6.20) 

The terms to the left side of the equal sign; 

x^ - A n x l _ B^u (6.21) 

contain only measured quantities, their derivatives, and the 
computed inputs u. Combining equation (6.18), and (6.20) 
results in an estimation equation; 

z = (A 22 -LA 12 )z + A 21 X 1 + L ^ X l~ A ll X l" B l U ^ + B 2 u (6.22) 
Equation (6.22) provides an observation of the unmeasured 
states, based on state measurements, the time derivative of 
the measurements, and the inputs. Additionally, the 
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measurement gain L appears to have the ability to specify 
the error dynamics. This equation is represented in block 
diagram form in figure 6-4. 

6.2.1 Adaptation for Implementation 

The resultant observation equation (6.22) meets the 
objective of controlling the rate at which the error 
converges, and eliminates the sensitivity to initial states 
as the process proceeds. The equation does, however, require 
the time derivative of the measured states. The time 
derivative of the measured states may be the variables it' is 





Figure 6-4. Observer with Measurement, and Measurement 


Derivatives 
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desired to estimate. This is indeed the case for the 
flexible arm. 

Figure 6-5 depicts an estimation system which does not 
require knowledge of the time derivative of the state 
measurements. This is accomplished by utilizing the 
following substitution. 

Lx x = (A 22 -LA 12 )Lx 1 (6.23) 

Insertion of this result into the estimation equation (6.22) 
yields; 

w = (A 22 -LA 12 )w + [(A 21 -LA 11 ) + (A 22 -LA 12 )L]x (6.24) 

+ (b 2 -lb 1 )u 


where , 


z = w + Lx^ 


(6.25) 


The motivation for this substitution is made more apparent 
by noting the adjustments made to figure 6-4 in deriving the 
observer shown in figure 6-5. This adjustment effectively 
pushes the time derivative of the measurement through the 
integration block. Gopinath[VI3 ] showed that the error 
dynamics remain unchanged. 


6.3 Application of Reduced Order Observers to Single Link 

Flexible Arms 

This section describes the application of the general 
reduced order observer to the single flexible manipulator. 




Reconstruction of the modal amplitudes is treated 

separately, and the following development considers these 
quantities as inputs for the estimation of the modal 
velocities . 

Following the earlier partitioning scheme for 

measured, x^, and unmeasured, x 2 , states the state vectors 
for the flexible manipulator can then be organized as; 

x x = [ 9 / <3^ • ^ 2 ' * * ' ^n ' ^ ^ (6.26) 

*2 = t • ^2 ’ * * * ^n ^ (6.27) 

where the requirement is to form an estimate z of the modal 
velocities contained in the x 2 state vector. This form is 
directly compatible with the state space formulation derived 



Figure 6-5. Adaptation for Implementation 
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in the dynamic modeling section. A conversion of the 
continuous estimation equations developed above to digital 
form appropriate for implementation in a micro-processor is 
accomplished by zero order hold equivalence. 


6.3.1 Specification of the Measurement Gain L 
The actual selection of the measurement gain matrix L 
for the flexible arm system is not as direct as that implied 
by a casual glance at the error dynamic equation (6.16). The 
estimation equation for the modal velocities of an n mode 
series is depicted in equation (6.28). 


w = [F]w + [L'Jx^ + [B']u (6.28a) 

F = A 22 -LA 12 (6.28b) 

L'= (A 21 -LA 11 )+(A 22 -LA 12 )L (6.28c) 

B' = B 2 -LB 1 ( 6 . 28d) 

z = w + Lx^ (6.28e) 


Where F the estimator dynamic matrix is 

nxn. Specification of estimator dynamic matrix F in equation 

2 

28a above results in n equations. The measurement gain 

2 

matrix L' , however, is nx(n+2) and will have n +2n terms. 
Thus, a specification of the error dynamics does not 
completely determine the elements of L. This will occur 
whenever more state measurements are made than there are 
states to be estimated. This allows significant freedom to 
the designer, and use of this freedom to improve system 


robustness will be discussed next. 


6.4 Pole Placement and Robust Observers 
The design freedom mentioned earlier can be used to 
increase the robustness of the observer system. By examining 
figure 6-6, a block diagram of the control implementation, 
it is apparent that the observer utilizes commanded torque 
as opposed to the actual torque. If the depicted system is 
broken at node A, which would correspond to the servo-amp 
for the motor turned off, the earlier discussion of poles 
for the combined observer/plant system does not apply. The 
poles are no longer separable, and the observer displays 



Figure 6-6. Implementation Block Diagram 








"closed loop" poles described by; 

A 22~ LA 12"^ LB l" B 2^ K 2 { 6 . 29 ) 
K 2 is a gain vector associated with modal velocities. These 
poles are not identified in the earlier discussion for 
observer design. Initial disturbances are readily available 
to this system via state measurement, and unstable poles 
quickly result in estimates which saturate the system. This 
results in an experimental system with a "hard start" 
behavior . 

Problems of a similar nature were discussed by Doyle 
[VI5], and the reduction of sensitivity to this problem was 
termed an increase in robustness. The equations for the 
closed loop observer poles are combined with the equations 
for the observer dynamic matrix for determining the elements 
of the measurement gain matrix L. 

6.5 Experimental Investigation 

Real-time experiments were conducted to investigate 
modeling assumptions, and observer design performance. The 
major issues arising during implementation result from the 
truncation of the modal series made to achieve a low order 
model, and hardware performance. Balas [VI 6] considered the 
possibility of control "spill-over" into the higher 
neglected modes having deleterious effects. Also, the 
proximity of the flexible poles to the imaginary axis makes 
the system intolerant of unmodeled phase terms introduced by 
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hardware [VI 7 ] . 

6.5.1 Control Algorithm 

A linear quadratic steady state regulator with a 
prescribed degree of stability was designed and implemented 
for a two mode model. The controller design was executed for 
a sample and hold system. An optimal regulator design was 
selected with gains large enough to contrast the step 
response performances of the different observer dynamic 
specifications. At low gains stability is hardly a problem, 
and at very large gains, component performance begins to 
cloud the observations. 

6.5.2 Measured Performance 

The first issue investigated was the impact of the 
cycle time of the controller. The reconstruction, 

observation, and control algorithm executed at roughly 178 
Hz, more than ten times the flexible frequency to be 

controlled, yet only twice the fourth modal frequency and 
four times the third. The first four clamped modes of the 
system are presented in table 6-1. 


Table 6-1. Natural Frequencies (Hz ) 


Mode 


Measured 

2.08 

13.92 

41.38 

81.18 


Calculated 

2.096 

13.989 

41.524 

81.225 


The effect of the controller cycle time was examined 
by considering the step responses of a collocated controller 
(sensor and actuator at the same location) using joint 
angle, and joint velocity executing at 500Hz, as shown in 
figure 6-7, and at the speed of observer/controller , 178Hz, 
shown in figure 6-8. The joint angle, and strain at the base 
of the beam were used to characterize the time response. The 
gains utilized were the same as for the joint angle and 
joint velocity of the optimal regulator. The longer cycle 
time associated with the 178Hz controller resulted in a 
noticeable increase in the excitation of the third flexible 
mode. The amplitude of the flexible vibration is not as 
dramatic as the strain response. 

Next, a zero order hold equivalent observer was 
designed with the discrete poles equivalent to negative real 
poles two and a half times the frequencies of the flexible 
poles being examined. The relationship between the flexible 
modes and the observer poles is shown in table 6-2. The 
result for this observer is shown in figure 6-9. The 
controller was most sensitive to the modal velocity 



Figure 6-8. 178Hz Collocated Contoller 
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Table 6-2. Relationship between Flexible Modes 

and Observer Poles 

Equivalent Continuous Pole 
Case 1 ( 2 . 5x ) Case 2 (5. OX) 

-5.2 -10.40 

-34.8 -69.60 

gains produced by the observer. The instabilities did not 
occur in the modes which were being treated by the truncated 
model, but in the modes truncated for the model. The fourth 
mode at 81Hz also had an increased response although this is 
not apparent in the response. This is due to measurements 
and control torques aimed at the first two frequencies 
"spilling" over into the higher modes. 


Modal Frequency 

2.08Hz 

13.92Hz 


.500 

rads 

150 

ue 


Joint 

Angle 

Strain 
at Base 


-500 

ye 

-.500 

rads 



Figure 6-9. Step Response Case 1. 


The measurement was repeated for an observer with 
poles at five times the flexible mode being treated. The 
relationship between the poles and the flexible modes is 
presented in table 2. The response for this 
observer/controller combination is shown in figure 6-10. 
This controller does a very good job of controlling the 
first two flexible modes, reducing the amplitude of the 
strain and quickly damping the vibration. The untreated 
third mode however is still noticeably excited, and the 
power spectrum indicated an increased excitation of the 
fourth mode. 



Figure 6-10. Step Response Case 2. 
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At higher gains, especially modal velocity gains even 
this observer yielded unstable results in the higher modes, 
though the controlled modes were consistently well damped. 
The indicated trend is to push the observer poles farther 
and farther to the left. Placing the faster observer pole 
equivalent to ten times the second flexible mode's pole 
results in characteristic times for the observer approaching 
the cycle time of the controller. The response for this 
observer/controller combination is depicted in figure 6-11. 
This combination resulted in significant excitation of the 
third mode, and for the first time a dramatic response in 
the fourth mode. This is counter to the trend, and most 



Figure 6-11. Step Response, Poles 10 x Mode 
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likely represents a fatal combination of increased noise 
sensitivity as the observer poles are pushed father to the 
left, and aliasing resulting from the controller cycle rate. 


6 . 6 Summary 

Reconstruction and observation of flexible variables 
for use in controlling a single link flexible arm has been 
successfully demonstrated. The results indicate that the 
negative real parts of the observer poles must be placed at 
least five times the magnitude of the flexible mode's 
frequency being estimated, and possibly faster for higher 
gains. Control spill-over was observed in several of the 
cases investigated, and was aggravated by slow observers to 
the point of unstable responses for some designs. A dominant 
factor in the design of high performance observers/ 
controllers for flexible systems appears to be the response 
of the higher modes. The application of passive damping 
[VI 7 ] , treating the neglected higher modes, may reduce the 
performance requirements of the observer /control system. 


CHAPTER VII 


OPTIMAL REGULATOR 


7.1 Introduction 

This section discusses the results of implementing an 
optimal linear state feedback controller based on quadratic 
criteria for the experimental arm. A steady state, or 
regulator, design with prescribed degree of stability was 
investigated experimentally. The control law K for the 
linear system described by, 

x = Ax + Bu (7.1) 

u = K T x (7.2) 


is selected so as to 
index of equation (7.3) 


PI = I 


minimized the quadratic performance 


[ x T Qx + u T Ru ] dt (7.3) 


over the process. The steady state gain solution is sought. 
The formulation and solution to this problem with a 
prescribed degree of stability, a, is discussed in detail in 
Appendix C, including modification for the discrete sampled 
and hold implementation. 

The regulator is implemented for two models which 
include a single flexible mode, and two flexible modes in 
addition to the rigid mode. Inclusion of more flexible modes 


hopefully increases the model accuracy and controller 
performance. Increasing the model order has a significant 
cost in cycle speed of the digital controller. The regulator 
which treats two flexible modes is discussed first. 

7.2 Two Flexible Mode System 

Based on initial estimates of computational 
capabilities, and initial experimental results examining 
model performance discussed in chapter 3, a base design 
treating the rigid motion, and two flexible modes associated 
with clamped-free vibration modes was selected. This 
selection of model order was supported by work which is 
recorded in chapters 3 and 4. The performance of a regulator 
based on this model is discussed in this section. 

7.2.1 Controller Design 

The parameters available to the designer affecting the 
performance of the LQR controller are the elements of the 
weighting matrices and the prescribed degree of stability. 
The initial weighting matrices utilized were taken from 
Sangveraphusiri's[VlIl] earlier design and simulation work. 
Varying the elements of the weighting matrices as a tool for 
achieving performance improvements of the controller was 
extremely difficult. There is not a direct or intuitive 
approach to modification of the elements of the matrices 
which relates to performance goals, except in a broad sense. 
Gains are easily increased by reducing penalties on 



actuation, but selectively altering velocity, or position 
state penalties had strange impacts on performance. 

The most useful method for altering the performance 
was found to be obtained by varying the prescribed degree of 
stability. This proved to result in consistent trends, 
compatible with design intuition. 

7.2.2 Implementation 

The controller was implemented in software described 
in appendix F. A cycle time of 5.9 milliseconds was achieved 
for the two flexible mode controller. The controller results 
are inexorably linked to the performance of the modal 
velocity observer. The implemented observer was that 
presented in chapter 6 with error dynamics five times faster 
than the open loop poles being estimated. Good results, 
predicted from controller designs based on the truncated 
model, were often not realized in experiment. The 
experimental system was extremely sensitive to the second 
flexible mode damping gain as discussed in chapter six. 

This limitation • on damping in the second mode, 
apparently due to available computational speed, prevents a 
rigorous examination of this controller. Controller design 
by the use of a prescribed degree of stability can be 
extended to act as a pole placement algorithm. Specification 
of the diagonal elements of the stability matrix associated 
with the each of the states of a specific mode, the minimum 
degree of stability for each mode can be affected 


independently. The stability matrix, resultant gains, and 
closed loop system eigenvalues for the first case to be 
discussed are presented in table 7-1. 

Use of the prescribed degree of stability as a pole 
placement algorithm allowed for evaluation of the "best" 
combination for the two mode controller. The second flexible 
mode was made to have as great a degree of stability as 
possible without introducing instabilities into the higher 
modes as noticed in the observer evaluation. The performance 
of the system was then evaluated as the degree of stability 
for the rigid, and first flexible mode was increased 
independently of the second flexible mode. 


7-1. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figures 7-1 and 7-2 

Stability Matrix 

a = diag[ 2.50 2.50 .2 2.50 2.50 .2] 

Continuous Gains 


F( collocated) 

= 111.2E+4 0. 

0 . 

474.1 0. 0. 

F( modal) = 

111.2E+4 -406.7 - 

291.6 

474.1 15.7 -6. 

Continuous Eigenvalues 



Collocated Controller 

Modal 

Controller 

-3.239 

3 . 053E-16 

-4.95 

-4.78E-12 

-11.98 

-3 . 026E-15 

-5.04 

4.86E-13 

-4.246 

+/-15.11 

-4.99 

+/- 18.97 

-.3071 

+/-88 . 99 

-.401 

+/- 89.06 

Discrete Gains 



F( collocated) 

= 871.9 0. 0. 

373. 

10 . 0 . 

F ( modal ) = 

871.9 -297.8 177. 

4 373. 

1 11.7 -5.3 

Discrete Eigenvalues 



Collocated Controller 

Modal 

Controller 

.962 

9 . 497E-18 

.969 

-.112 

.975 

-5 . 002E-15 

.969 

.112 

.969 

+/-9.539E-2 

.976 

+/-.0112 

.863 

+/-5.009E-1 

.863 

+/-.5008 
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7.2.3 Experimental Results 

The first experiment conducted in this stage of the 
work was to compare a collocated controller to a modal 
controller by implementing the same joint variable gains in 
each case. A step response in joint angle was commanded, and 
the response measured. The response figures all contain two 
parts, part a depicts joint angle time response and motor 
current, part b presents strain measurements from the base 
and midpoints of the beam. The current represents torque 
applied at the joint, and the strain is a measure of the 
bending due to the flexible modes. 

The degree of stability was the parameter varied for 
this test. The prescribed degree of stability was increased 
by increments of .25 until the stability of the collocated 
controller, started to degrade, as shown in figure 7-1 
(degraded response being an increase in the flexible mode 
response). The closed loop eigenvalues for the collocated 
case and modal controller are shown in table 7-1. Further 
increases of the prescribed degree of stability resulted in 
instabilities of the third flexible mode, for the collocated 
controller. 

A modal controller with the same prescribed degree of 
stability in the rigid and first flexible mode, with limited 
stability in the second flexible is shown in figure 7-2. 

The increased stability of the higher mode is readily 
apparent in the strain response. After the large initial 



transient, the torque response tries to actively control the 
flexible vibration. Note however that the flexibility is not 
fully controlled; this is thought to be due in part to 
hysteretic friction of the joint. 

The degree of stability was increased for the rigid 
and first flexible mode, resulting in higher gains which 
would hopefully overcome the joint friction. The step 
response is repeated as shown in figure 7-3. The addition of 
the modal controller allows higher gains on the joint 
variables than was allowed for the collocated controller 
alone with this cycle time. The gains, and eigenvalues from 
the design process are shown in table 7-2. Note the 
increased damping of the 




Figure 7-la. Collocated Controller, 
a = diag[ 2 . 50 2.50 .2 2.50 2.50 .2], 
Joint Angle/Motor Current Step Response 



Figure 7-lb. Collocated Controller, 
Strain at Base/Midpoint Step Response 



Figure 7-2a. Modal Controller, 
a =diag[2.50 2.50 .2 2.50 2.50 .2], 
Joint Angle/Motor Current Step Response 



Figure 7-2b. Modal Controller, 
Strain at Base/Midpoint Step Response 
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Figure 7-3a. Modal Controller, 

• a - diag[ 3 . 25 3.25 .2 3.25 3.25 .2], 

Joint Angle/Motor Current Step Response 



Figure 7-3b. Modal Controller, 
Strain at Base/Midpoint Step Response 



76 


Table 7-2. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figure 7.3 

Stability Matrix 

a = diag[ 3.25 3.25 .2 3.25 3.25 .2] 

Continuous Gains 

F ( modal ) = 1964.9 -699.1 -318.4 668.1 31.6 -1.83 
Discrete Gains 

F ( modal ) = 1523.2 -494.8 -34.5 520.8 23.6 -1.68 

Continuous Eigenvalues Discrete Eigenvalues 

Modal Controller Modal Controller 


-6.44 

-7.49E-12 

.961 

-.113 

-6.55 

7.31E-12 

.961 

.113 

-6.50 

+/- 18.99 

.969 

+/-.0144 

-.399 

+/- 89.06 

.863 

+/-.5008 


flexible mode in the response. This controller implemented 
with this prescribed degree of stability, however, appeared 
to be sensitive to noise and would often go into a steady 
vibration of limited amplitude. A second step command or 
touch of the hand would often stop the vibration, but 
marginal stability had been achieved in the fourth flexible 
mode at roughly 83hz. Once the beam vibration was stopped by 
use of the hand, it would remain motionless until perturbed. 
This change is most likely caused by a change in the joint 
damping from viscous friction, (when it is in motion) , to 
coulomb friction as it stops. The step response is captured 
again for a case where this steady vibration is sustained 
over the interval in figure 7-4. 

Further increases in the prescribed degree of 
stability yielded similar instabilities in the higher 
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Figure 7.4a. Modal Controller, Large a, 
a = diag[ 3 . 25 3.25 .2 3.25 3.25 .2], 

Joint Angle/Motor Current Unstable Step Response 
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Figure 7.4b. Modal Controller, Large a. 
Strain at Base/Midpoint Unstable Step Response 
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Figure 7. 5a. "Best" Modal Controller, 
a — diag[ 3 . 25 ? .2 3.25 ? .25], Increased Modal Amplitude 
Gain, Joint Angle/Motor Current Step Response 



Figure 7.5b. "Best" Modal Controller, Increased Modal 
Amplitude Gain, Strain at Base/Midpoint Step Response 




flexible modes. The parameter observed to cause this affect 
was the damping gain on the first flexible mode. Trial and 
error indicated that higher modal amplitude gains resulted 
in a more stable response for the same modal damping gain. 
This trend of modal stiffness gain increasing system damping 
is supported by work done in the single mode section. 

The modal amplitude gain was increased until a "best" 
(most stable with respect to disturbances) response was 
obtained for the same first flexible mode damping gain. This 
response is shown in figure 7-5, and the design parameters 
presented in 7-3. Further increases in modal amplitude gain 
resulted in higher levels of excitation in the untreated 
modes. 

The modal controllers did improve the response of the 
flexible mode, and in the latter cases did yield 
satisfactory control over the first flexible mode. 
Additionally, with the modal controller larger gains could 

Table 7-3. Design Results, Stability Gain Vectors, 

Closed Loop Eigenvalues for Figure 7-5 

Stability Matrix 
a = diag[ 3.25 ? .2 3.25 ? .2] 

Discrete Gains 

F( modal) = 1523.7 -900.0 34.5 520.8 23.5 -1.68 

Discrete Eigenvalues 
Modal Controller 
.970 - . 303E-17 

.976 . 175E-17 

.969 +/-.0884 

.908 +/-.4137 
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be applied to the joint variables than was possible with the 
collocated controller. The next question to be addressed is 
the robustness of these control laws. The next section 
investigates the noise sensitivity, and payload sensitivity. 
7.2.4 Disturbance Rejection and Robustness 

In the preceding section control laws were selected 
which yielded faster settling times in the design process by 
effectively moving the continuous closed loop poles farther 
and farther into the left half plane using the prescribed 
degree of stability. This results in larger and larger gain 
values. This section will investigate the disturbance 
rejection and robustness of the control laws, examining 
what, if any, penalties are incurred with the higher gain 
values. First disturbance impulses will be applied to the 
experimental system, and then changes in payload mass from 
the design value will be examined. 

7. 2. 4.1 Disturbance Impulse Response 

An impulse hammer was used to apply a disturbance to 
the beam at the payload. This would correspond to the arm 
making contact with a work piece or bracing surface. The 
amplitude of the impulse was selected to cause a peak 
disturbance of roughly five to six inches at the payload. 
The time domain response of the beam was found to be the 
most revealing measurment, though frequency domain results 
could be obtained from the hammer impulse signal. Though the 
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signal from the hammer was not utilized in these 
measurements, the design of the hammer still enhanced the 
operators control of the impulse delivered to the beam. The 
response of the collocated control law of 7-1 above is shown 
in figure 7-6 

The beam was additionally struck at random locations 
along its length with similar responses. The response of the 
modal control law with the same prescribed degree of 
stability on the joint and first mode state variables is 
shown in figure 7-7. 

The response of modal control law which achieved the 
highest prescribed degree of stability, (design data in 
table 7-2), is shown in figure 7-8. 

The modal control law with a high degree of prescribed 
stability, and the "best" control law, while able to control 
the flexible vibration had large peaks and a good deal of 
activity in the impulse response. This indicates that the 
tighter control laws with large gain values are more 
susceptible to disturbances. 
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Figure 7-6a. Collocated Controller, Disturbance Impulse, 
Joint Angle/Motor Current Response 



Figure 7-6b. Collocated Controller, Disturbance Impulse, 
Strain at Base/Midpoint Response 
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Figure 7-7a. Modal Controller, Disturbance Impulse, 
• Joint Angle/Motor Current Response 



Figure 7-7b. Modal Controller, Disturbance Impulse 
Strain at Base/Midpoint Response 
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Figure 7 -8a. Large a Modal Controller, Disturbance Impulse, 
Joint Angle/Motor Current Response 
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Figure 7-8b. Large a Modal Controller, Disturbance Impulse, 
Strain at Base/Midpoint Response 



1.2. A. 2 Payload Sensitivity 


The ability of the controller to tolerate variations 
in payload mass is of interest in real applications where a 
variety of objects must be handled. The payload was 
increased to a total of four times the design payload, and 
the step response observed again. Responses for the control 
laws are shown in figures 7-10, through 7-14. 

The additional payload mass resulted in increased 
overshoot of the joint angle, and modal variables typical of 
the expected reduction in damping ratio. Additionally, the 
response of the higher untreated flexible modes was greatly 
reduced in all cases, even the case of the modal controller 
with a large prescribed degree of stability. This is due to 
increased separation of the higher frequencies due to the 
variation in boundary condition at the payload end of the 


beam. 
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Figure 7-10a. Collocated Controller, 4 Times Payload, 
Joint Angle/Motor Current Response 



Figure 7-10b. Collocated Controller, 4 Times Payload, 
Strain at Base/Midpoint Response 





Figure 7-lla. Modal Controller, 4 Times Payload, 
Joint Angle/Motor Current Response 



Figure 7-llb. Modal Controller, 4 Times Payload, 
Strain at Base/Midpoint Response 





Figure 7-12a. Large a Modal Controller, 4 Times Payload, 
Joint Angle/Motor Current Response 



Figure 7-12b. Large ct Modal Controller, 4 Times Payload, 
Strain at Base/Midpoint Response 
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Figure 7-13a."Best" Modal Controller, 4 Times Payload, 
Joint Angle/Motor Current Response 



Figure 7-13b."Best" Modal Controller, 4 Times Payload, 
Strain at Base/Midpoint Response 
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7.3 Single Flexible Mode System 
Computational speed, and not model order, appeared to 
be the limiting factor in many of the experiments conducted. 
Therefore the design and implementation were repeated for a 
system model comprised of the rigid mode, and one flexible 
mode. The first clamped-mass mode, shape was selected for 
inclusion in the model. 

7.3.1 Design and Implementation 

Once again the prescribed degree of stability proved 
to be the fastest and most flexible method for placing the 
system poles. However the low frequency of the first 
flexible mode and increased computing speed allowed for a 
placement of the single observer pole equivalent to ten 
times the frequency of the modal velocity being estimated. 

The modal reconstruction/observer/control law 
combination for the single mode case operated at a cycle 
time of 4.8 milliseconds. 

7.3.2 Experimental Results, Prescribed Degree of Stability 
The first experiment conducted in this stage of the 

work was to compare the performance of the collocated 
controller operating at the frequency of the faster one mode 
controller to the earlier collocated response. The same 
collocated gains as those used for the controller with two 
flexible modes was applied to the single flexible mode 
system. The step response is shown in figure 7-14. 


There is a marked reduction in the excitement of the 


third flexible mode when compared to the collocated 
controller operating at the slower cycle time of the two 
mode system. A single flexible mode controller with the same 
prescribed degree of stability as the collocated case was 
implemented, and the step response is shown in figure 7-15. 
The single mode controller achieves a smoother more damped 
response over the two mode controller primarily because of 
the increased speed of the observer and sampling frequency. 

The prescribed degree of stability for the collocated 
controller operating at the speed of the faster single mode 
controller could be increased significantly over that of the 
slower two mode system before the same amount of excitement 
was observed in the third flexible mode. Additionally at 
the higher gains more damping was observed after the initial 
step transient in the first flexible mode. This is thought 
to result from the higher control actions overcoming the 
hysteretic friction of the joint. The response for this 
collocated controller is shown in figure 7-16. 
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Figure 7-14a. Collocated Controller, 
a = diag[2 . 5 2.5 2.5 2.5], 

Joint Angle/Motor Current Step Response 


Strain at Base 







Figure 7-15a. Modal Controller, 
a = diag[2.50 2.50 2.50 2.50], 

Joint Angle/Motor Current Step Response 



Figure 7-15b. Modal Controller, 
Strain at Base/Midpoint Step Response 



Figure 7-16a. Tighter Collocated Controller, 
a = diag[ 3 . 0 3.0 3.0 3.0] , 

Joint Angle/Motor Current step Response 



96 


A modal controller for the first flexible mode was 
implemented with the same prescribed degree of stability in 
both the rigid and flexible mode as that for the collocated 
controller. This response is shown in figure 7-17. Note 
again the success of the modal controller at reducing the 
excitation of the first flexible mode, and the higher 
flexible modes, not treated by the model until the amplitude 
gets very small. The design data for the controllers of 
figures 7-14,15,16,17 is presented in tables 7.4-5 


Table 7-4. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figures 7-14 and 7-15 


Stability Matrix 
a = diag[ 2.50 2.50 2.50 2.50] 


Continuous Gains 

F(collocated) = 1112. 0. 474.1 0. 

F( modal) = 1112. -406.7 474.1 15.7 


Continuous Eigenvalues 

Collocated Controller 


-3.215 -3 . 306E-16 
-12.45 1 . 669E-16 
-4.333 +/-15.11 


Modal Controller 
-4.434 2 . 305E-15 

-5.744 -3 . 199E-15 

-5.203 +/-19.01 


• Discrete Gains 

F(collocated) = 871.9 0. 373.1 0. 

F( modal) = 871.9 -297.8 373.1 11.7 


Discrete Eigenvalues 

Collocated Controller 


967 

981 

975 


1 . 536E-18 
-1 . 570E-28 
.0778 


Modal Controller 
.959 +/-.152 

.992 +/-.106 




Figure 7-17a. Tighter Modal Controller, 
a - diag[3.0 3.0 3.0 3.0], 

Joint Angle/Motor Current Step Response 



Figure 7-17b. Tighter Modal Controller, 
Strain at Base/Midpoint Step Response 
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Table 7-5. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figures 7-16 and 7-17 

Stability Matrix 
a = diag[ 2.75 2.75 2.75 2.75] 

Continuous Gains 

F( collocated) = 1346.4 0. 527.0 0. 

F( modal) = 1346.4 -470.9 527.0 21.1 

Continuous Eigenvalues 

Collocated Controller 
-3.327 -3 . 341E-16 

-16.11 2.435E-15 

-4.065 +/-14.388 

Discrete Gains 
F(collocated) = 1294.3 0. 509.7 0. 

F ( modal ) = 1294.3 -419.2 509.7 19.2 

Discrete Eigenvalues 

Collocated Controller Modal Controller 

.928 9 . 380E-18 .970 +/-.320E-16 

.983 -7.735E-19 .969 +/-.089 

.976 +/-.0684 


The prescribed degree of stability was increased by 
increments of .25 until until the largest degree resulting 
in a stable response was achieved. The response for the 
resulting control law is presented in figure 7-18. This 
controller results in large applied torques during the 
initial transient and an increased level of noise throughout 
the response in the torque and strain traces. The stability 
and damping of the first flexible mode is definitely 
increased, however the settling time for the system has not 
been significantly increased. The large torques applied to 
slew the joint variables at high gains causes large flexible 
deflections which, although highly damped, require time to 


Modal Controller 
-5.50 +/-.0065 

-5.50 +/-19.13 




Figure 7-18a. Large a Modal Controller, 
a = [3.75 3.75 3.75 3.75], 

Joint Angle/Motor Current Step Response 



Figure 7-18b. Large a Modal Controller 
Strain at Base/Midpoint Step Response 






settle out. The design results for this case are shown in 
table 7-6. 

Table 7-6. Design Results, Stability Gain Vectors, 

Closed Loop Eigenvalues for Figure 7-18 

Stability Matrix 
a = diag[ 3.75 3.75 3.75 3.75] 

Continuous Gains 

F( modal) = 2668.0 -840.3 806.2 47.7 
Discrete Gains 

F( modal) = 2528.3 -758.4 770.2 43.3 

Continuous Eigenvalues Discrete Eigenvalues 

Modal Controller Modal Controller 

-7.50 +/- .0049 .958 +/-.149E-17 

-7.50 +/- 19.13 .960 +/-.0877 

7.3.3 Experimental Results , Pole Placement 

Increasing the prescribed degree of stability by equal 
amounts for both the rigid and flexible modes did not 
achieve the desired result of improving the settling time 
for the system. Although large amounts of damping was 
successfully introduced into the flexible mode the overall 
settling time did not consistently improve. Large overshoots 
of joint angle were necessary to reduce the excitement of 
the flexible mode, and the large gains associated with high 
degree of stability introduced noise into the system. 

This indicated that better results might be obtained 
if more stability was prescribed for the flexible mode than 
the rigid mode. A prescribed degree of stability for the 
joint angle was selected which achieved a rapid slew rate in 
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•4 


the earlier experiments, yet which did not tend excite the 
flexible mode as much as the largest degrees of stability 
prescribed for the joint variables. The stability for the 
flexible mode was then moved as far left as would allow for 
a stable response. This result is shown in figure 7-19, and 
the design data in table 7-7. 


Table 7-7. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figure 7-19 

Stability Matrix 
a = diag[ 2.75 3.50 2.75 3.50] 

Continuous Gains 

F( modal) = 1372.0 -736.4 553.1 14.2 


Discrete Gains 

F ( modal ) = 1306.8 -664.1 529.1 11.6 


Continuous Eigenvalues 
Modal Controller 
-5.22 +/- 1.00 

-7.27 +/- 19.46 


Discrete Eigenvalues 
Modal Controller 
.961 +/-.089 

.974 +/-.0042 


+ 
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Figure 7-19a. Modal Controller, 
a = diag[ 2. 75 3.5 2.75 3.25], 

Joint Angle/Motor Current Step Response 



Figure 7-19b. Modal Controller, 
Strain at Base/Midpoint Step Response 
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An interesting result of this design procedure is that 
samller amounts of modal damping are required to move the 
flexible pole to the left than when both the rigid and 
flexible are moved to the left in equal amounts. However, 
the resultant trace shown in figure 7-19 does not show the 
predicted damping. The modal damping was increased to 
observe its effect, and this result is shown in figure 7-20. 


Table 7-8. Design Results, Gain Vectors, 
Closed Loop Eigenvalues for Figure 7-20 

Discrete Gains 

F ( modal ) = 1306.7 -664.0 529.9 43.2 


Continuous Eigenvalues 
Modal Controller 
-4.56 +/-1.61 

-3.88 +/- 21.96 


Discrete Eigenvalues 
Modal Controller 
.978 +/-7.40E-3 

.976 +/-.104 


The additional damping resulted in a very nice 
response, with the shortest settling time and least 
overshoot of all the designs. The resultant design 
parameters are contained in table 7-8. Two additional 
conditions are thought to be of interest in the identifying 
the behavior of this combination. One case is the largest 
possible stable combination of amplitude and rate gain on 
the flexible mode. The second case is the application of 
modal velocity gain alone to the flexible mode. The case 
where large modal gains are coupled with moderate joint 
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Figure 7-20a. Increased Modal Damping, Modal Controller, 
Joint Angle/Motor Current Step Response 



Figure 7-20b. Increased Modal Damping, Modal Controller, 
Strain at Base/Midpoint Step Response 
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variable gains is depicted in figure 7-21. This is seen to 
result in excitement of the second flexible mode. 


Table 7-9. Design Results, Stability Gain Vectors, 
Closed Loop Eigenvalues for Figure 7-21 

Discrete Gains 

F( modal) = 1306.7 -2000.0 529.9 50.0 


Continuous Eigenvalues 
Modal Controller 
-2.71 +/-2 . 53 

-4.91 +/-28 . 62 


Discrete Eigenvalues 
Modal Controller 
.969 +/.1364 

.976 +/-.012 


Two cases of modal velocity gain only on the flexible mode 
coupled with reasonable joint variable gains were selected. 
Figure 7-22 depicts the response for a large amount of 
damping gain which achieved a stable response. Note that the 
first flexible mode while damped, is not damped as well as 
when coupled with some amplitude gain. 


Table 7-10. Design Results, Gain Vectors, 
Closed Loop Eigenvalues for Figure 7-22 

Discrete Gains 

• F( modal) = 1306.7 -0.C 529.9 40.0 


Continuous Eigenvalues 
Modal Controller 


-3.41 

-9.95 

-2.13 


3.6E-16 
1.70E-17 
+/- 18.36 


Discrete Eigenvalues 
Modal Controller 
.951 1.33E-18 

.984 -1.76E-19 

.985 +/-.087 
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Figure 7-21a. High Stif f ness , Modal Controller, 
Joint Angle/Motor Current Step Response 



Figure 7-21b. High Stiffness, Modal Controller 
Strain at Base/Midpoint Step Response 


3 . 59 
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Figure 7-22a. Modal Damping Only, Modal Controller, 
• Joint Angle/Motor Current Step Response 



Figure 7-22b. Modal Damping Only, Modal Controller, 
Strain at Base/Midpoint Step Response 
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The second case of damping gain only on the flexible mode 
coupled with moderate gains on the joint variables is shown 
in figure 7-23. This time the modal damping gain was 
increased over the last case, and a pinned-free vibration 
mode was excited. 


Table 7-11. Design Results, Gain Vectors, 
Closed Loop Eigenvalues for Figure 7-23 


Discrete Gains 

F( modal) = 1306.7 -0.0 529.9 -50.0 


Continuous Eigenvalues 
Modal Controller 
-3.13 2.12E-17 

-2.94 0.0 

-3.29 +/- 10.7 


Discrete Eigenvalues 
Modal Controller 
.851 -8.67E-19 

.985 -9.37E-19 

.983 +/-.050 



Figure 7-23b. Large Modal Damping, Modal Controller, 
Strain at Base/Midpoint Step Response 
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7.3.4 Disturbance Rejection and Robustness 
Two of the modal controllers from the previous section 
attained results consistent with the aims of the design 
process. The modal controller with the high prescribed 
degree of stability in both the rigid mode and the flexible 
with step response shown in figure 7-18 is examined in this 
section. Additionally, the controller with a larger 
prescribed degree of stability in the flexible mode (and 
higher damping gain than from the design process) with step 
response figure 7-20 is examined here. 

7 . 3 . 4 . 1 Disturbance Impulse Response 

An impulse hammer was used to apply a disturbance to 
the beam at the payload. This would correspond to the arm 
making contact with a work piece or bracing surface. The 
amplitude of the impulse was selected to cause a peak 
disturbance of roughly five to six inches at the payload. 
The time domain response of the beam was found to be the 
most revealing measurment, though frequency domain results 
could be obtained from the hammer impulse signal. Though the 
signal from the hammer was not utilized in these 

measurements, the design of the hammer still enhanced the 
operators control of the impulse delivered to the beam. The 
disturbance response of the modal controller with a large 
and equal amount prescribed stability is shown in figure 7- 
24, and the disturbance response of the controller with a 
larger degree of prescribed stability in the flexible mode 
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Figure 7-24a. Large a Modal Controller, Disturbance Impulse 
Joint Angle/Motor Current Response 
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Figure 7-24b. Large a Modal Controller, Disturbance Impulse 
Strain at Base/Midpoint Response 
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Figure 7-25a. Increased Modal Damping, Modal Controller, 
• Joint Angle/Motor Current Disturbance Response 
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Figure 7-25b. Increased Modal Damping, Modal Controller 
Strain at Base/Midpoint Disturbance Response 



is shown in figure 7-25. The large gains associated with 
prescribing a large degree of stability reduces the 
disturbance rejection of the system, as seen in the trace. 

7 . 3 . 4 . 2 Payload Sensitivity 

The payload was increased to a total of four times the 
design payload. Responses for the two cases are shown in 
figures 7-26, through 7-27. The increased mass results in 
the expected overshoot of the rigid mode. The response of 
the unteated higher flexible modes was generally reduced in 
the experimental observations. This is thought to result 
from the increased separation of the modal frequencies. The 
higher modes approach clamped-pinned shapes while the lower 
modes do not change significantly. 
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Figure 7-26a. Large a Modal Controller, 4 Times Payload, 
Joint Angle/Motor Current Response 



Figure 7-26b. Large a Modal Controller, 4 Times Payload, 
Strain at Base/Midpoint Response 


L5 



Figure 7-27a. Increased Modal Damping, Modal Controller, 
4 Times Payload, Joint Angle/Motor Current Response 







Figure 7-27b. Increased Modal Damping, Modal Controller, 
4 Times Payload, Strain at Base/Midpoint Response 
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7 . 4 Summary 

Control laws designed as linear quadratic regulators 
did successfully damp and control the vibratory modes 
included in the system model. The controllers were even 
successful in the presence of joint friction and measurement 
noise . 

Using the prescribed degree of stability in the 
regulator design process allowed for rapid and direct 
placement of the model poles. 

Attaining a rapid cycle time for the digital control 
implementation by neglecting the second flexible mode 
resulted in an improved response over the inclusion of the 
second mode. 

Even small amounts of modal feedback resulted in more 
stable systems than collocated controllers, though the 
stability was added to untreated modes. 

Attempts to obtain very short settling times for the 
system by prescribing large degrees of stability were 
ineffective. Though the rigid and flexible modes appeared to 
individually achieve tighter control, coupling between the 
modes caused the overall time to remain roughly the same. 
Fast joint rotations caused large modal amplitudes to be 
occur, and the large modal gains resulting from the large 
degree of stability required additional joint angle 
rotations to damp the mode. 



The higher gains resulting from large prescribed 
degrees of stability also resulted in excitation of the 
higher untreated flexible modes, and poorer disturbance 
rejection. The best combinations resulted from placing more 
stability in the flexible mode, than the rigid mode. 

Large gains always resulted in the appearance of 
untreated flexible modes. It was anticipated that trying to 
move the flexible poles far from the modeled poles would 
result in reductions of model accuracy. This was indeed the 


observed result 
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CHAPTER VIII 


KALMAN FILTER 


8.1 Introduction 

This section discusses the application of a Kalman 
Filter to estimate the modeled states. The estimated states 
are then used to generate control torques for the 
experimental arm based on a LQR state feedback law. 

For a linear system subject to white noise plant 
disturbances w(t) and white measurement noises v(t). 


; i+l * **1 + Bu i + w i 

(8.1) 

Yi = Cx x + v x 

(8.2) 

T 

u. = rx. 

(8.3) 



Figure 8-1. Kalman Filter Block Diagram 










a filter can be designed with the structure shown in figure 
8-1 which minimizes the mean error. 

The task is to determine a steady state filter gain, L, 
which minimizes the performance index, 

PI = 1/2^ [e i T M -1 .e i + (y i -Cx i ) T R v " 1 i (y i -Cx i ) ] (8.4) 

M is the covariance of the filter error between the updated 
estimate, and the plant. R v is the covariance of the 
measurement noise. The formulation and solution to this 
problem is discussed in detail in Appendix D. 

8.2 Implementation 

The Kalman Filter discussed above was implemented for 
a model with a rigid mode and one clamped-mass flexible 
mode. Software for the IBM Series/1 executed at a cycle time 
of 5.85 milliseconds, slightly faster than that for the two 
mode observer yet significantly slower than the single mode 
observer. One of the reasons for the favorable speed 
compared to the two mode observer is the removal of the 
reconstruction algorithm. The measurement matrix include the 
strain measurements directly as shown in equation (8.5). 
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0.0 

0.0 


0.0 

c 9 2 d) ( 0 ) 

. 2 
3n 

c d 2 <j ( .5) 

. 2 
3ti 

0.0 


0.0 

0.0 

0.0 

1.0 


0.0 

0.0 

0.0 

0.0 


(8.5c) 


8 . 3 Measurement Noise 

The measurement gain, L, for the Kalman Filter depends 
entirely on knowledge of the covariances of the measurement, 
and plant disturbances. It is difficult to gain a thorough 
knowledge of the disturbances to the plant, especially in 
this case where it is hoped that unmodeled plant dynamics 
may be treated as disturbances. Information about the 
measurement noise is easier to gain and forms the starting 
point for this investigation. 

The power spectrums for the joint angle, joint angle 
velocity, strain at the hub, and strain at the midpoint of 
the beam were captured using a Hewlett Packard 3562A 
spectrum analyzer. 

The noise was thought to be bandlimited by a 
characteristic time of less than two seconds. The noise 
variance can be determined to within 10 percent by taking 
autocorrelation measurements over sampling intervals of at 
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least 100 seconds as given by; 


T = - 


( 8 . 6 ) 


where the .1 represents the needed accuracy, and 6 the 
limiting time. This is an approximation for Gauss-Markov 
correlation functions [villi]. 

The joint angle and beam tip were fixed to prevent 
rotations and vibration, and the power spectrums averaged 
over ten periods. Additionally, the sampling time was 
doubled to avoid wrap around, as suggested in the operating 
manual for the spectrum analyzer [VIII2]. This required 
roughly thirty minutes of data collection for each 
measurement . 

The resultant power spectrums were inverse Fourier 
transformed to obtain the autocorrelation function, and 
estimate of the variance. The power spectrums for the joint 
angle, figure 8.2a, and strain at the base, figure 8.2b, are 
presented as examples of the noise spectrum. The 
corresponding autocorrelation functions are shown in figures 
8.3a, and 8.3b. 









The estimates of the measurement noise variances based on 
the autocorrelation functions are shown in table 8.1. 


Table 8-1. Noise Variance Estimates Based on 
Autocorrelation Measurements 


Measurement 
Joint Angle 
Joint Velocity 
Strain at Base 
Strain at Midpoint 


Variance Estimate 

1.1E-6 rad^/sec 

238E-6 rad^/sec 3 

141E-9 Strain^/sec 
2 

936E-9 Strain /sec 


8.4 Filter Design 

The parameters available to the Kalman Filter designer 
are the measurement noise specifications, the elements of 
the disturbance noise distribution matrix, G, and the matrix 
of disturbance noise intensities I shown in equation (8.6). 
The covariance of the noise w^ for the digital 

implementation is determined from the distribution matrix, 
G, and the continuous noise, w(t) with intensities I, as 
shown in equation (8.7). 

x = Ax + Bu + Gw (8.6) 


2k " 


eAi Vi eAT “ + 


r At _ 

e AT GIG T e A T dx 


(8.7) 


The character of the distribution matrix, G, and 
intensity matrix, I, are often extremely difficult to 
determine [VIII3]. The untreated modes couple to the system 
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through the mass and stiffness matrices, as well as the 
control inputs by corrupting the strain measurements. 

Attempting to model the untreated modes as noises, and 
selection of an appropriate distribution matrix, G, for this 
case results in a tremendous parameter space to search. The 
following strategy [VIII3] has been suggested as one method 
for tackling the Kalman Filter design problem, and was 
partially utilized by Schmitz [VIII4] in his experimental 
work. A state estimator robust with respect to errors in 
modeling plant parameters can be obtained. The first step is 
to assume that the disturbances to the plant are caused by 
the control and are distributed by the input matrix, B. 
Next, the covariance of the plant disturbance is assumed to 
be much greater than the covariance of the measurement 
noise. This results in a robust state estimator with poles 
close to the poles of the plant. This strategy was examined 
experimentally. The parameters and design results are 
contained in table 8-2. 

The control laws resulting from LQR design were oupled 
with the Kalman Filter estimates of the states to form a 
closed loop system. Separation of the poles is assured, as 
shown for the state observer in chapter 6. Use of these 
control laws additionally provides a point of comparison 
with the reduced order observer/reconstruction controller 
discussed in chapter 7. 
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Table 8-2. Kalman Filter Design Results 


State Vector 


x i = [ 0, q, 6, g ] 


Measurement Vector = [ 9, e(0), e(.5), 8 ] 
Measurement Matrix (strain measured in micro strain) 


C = 


1.0 0.0 0.0 0.0 

0.0 1530.0 0.0 0.0 

0.0 630.0 0.0 0.0 

0.0 0.0 1.0 0.0 


Measurement Noise Covariance Matrix 
V = diag[ 1.09E-6 .145 .939 2.37E-4] 

Plant Noise Covariance 
Q = 1.0E+6 

Measurement Gain 


L = 


4.318E-2 

-3.552E-5 

4.142E-1 

■1.322 


-4.238E-5 

2.412E-4 

-2.178E-3 

9.902E-3 


■2.621E-6 

1.491E-5 

-1.346E-4 

6.122E-4 


1.914E-3 

-8.4341-3 

9.492E-1 

-4.416 


Discrete Eigenvalues (A-LC) 

.2386 

.4231 

.9773 +2.42E-2 

.9773 -2.42E-2 
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8 . 5 Experimental Results 

Initially the collocated control law of table 7-1 was 
applied to the Kalman Filter state estimates. This control 
law was stable for the two mode controller whose cycle time 
was roughly the same. Coupling this control law to the 
Kalman Filter resulted in growing oscillations of the third 
flexible mode during experiments with this control law. 

Another control law was designed with a reduced degree 
of prescribed stability and applied to the estimated states. 
The step response for this control law is shown in figure 8- 

4. The closed loop feedback back poles are identified in 
table 8-3 for the collocated, and modal control law. 

Note the slow decay of the 43Hz oscillation, and the 
overshoot of this controller. The process was repeated with 
a modal controller with the same prescribed degree of 
stability. The response for this case is shown in figure 8- 

5. There is very little alteration of the response; the 
Kalman Filter is obviously not tracking the states of the 
system very well. 


L28 




Figure 8-4b. Collocated Control Law, Strain at Base, 
Strain at Midpoint Response. 
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From experience it was apparent that the overshoot 
could be reduced by forcing the Kalman Filter to track the 
joint angle more quickly. The element of the gain matrix 
L( 1 , 1 ) which is the update of joint angle estimate due to 
joint angle measurement was increased ten fold. The step 
response was repeated with the modified measurement gain. 
The result is recorded in figure 8-6. 


Table 8-3. Closed Loop Poles. 

Collocated Feedback Law 
F = [ 667. 0. 349. 0.] 

Modal Control Law 
F = [ 667. -220. 349. 9. ] 


Discrete Eigenvalues 


Collocated Control Law 

Modal 

Control Law 

.957 

3 . 323E-18 

.973 

-1 . 086E-18 

.983 

-4 . 381E-18 

.978 

+3 . 782E-20 

.969 

+/-9 . 700E-02 

.970 

+/-1 . 100E-01 
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Figure 8 -5b. Modal Control Law, Strain at Base 
Strain at Midpoint Response. 





-325 
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Figure 8 -6b. Modified Measurement Gain, Strain at Base, 


Strain at Midpoint Response. 
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The results of this intuitive change in the measurement gain 
is readily apparent. It much more difficult to cause this 
type of change through creating "fictitious" noise values in 
the measurement covariance. 

Several other plant covariance values were examined, 
with similar results. It quickly became apparent that this 
was not a suitable approach to estimator design. 

8.6 Summary 

Comparison of the experimentally determined noise 
covariances to the levels of periodic excitations caused by 
the truncated higher flexible modes indicates that the 
control problem is dominated by truncation effects, as 
opposed to stochastic noise. The experimentally determined 
noise covariances result in expectations of very small 
errors from the these sources. This is particularly true in 
light of the measured content of the untreated flexible 
modes during the control experiments conducted in chapter 7 . 
Some strain traces presented in chapter 6 contained periodic 
amplitudes due to the higher flexible modes on the order of 
100 microstrain. The joint angle trace in many responses was 
also significantly corrupted by the higher modes. Thus the 
impact of the unmodeled modes in the system is far more 
important than a consideration of the system noise. 
Additionally, approximation of the impact of the distinct 
modal frequencies on the system by white noise covariance 



estimates is extremely questionable. 

For some systems, such as airplanes, modeling flutter 
of lift surfaces as stochastic disturbances to the craft is 
more appropriate. The flutter can be measured or rationally 
approximated, as well as the manner in which the disturbance 
propagates through the system dynamics. Additionally, the 
aircraft disturbances are typically at much higher 
frequencies than the characteristic eigenvalues of the craft 
[VIII5]. This is not the case for the flexible manipulator. 

Work done in chapter 6 indicated that observer poles 
for the estimation of the modal velocities needed to be much 
faster than the system poles being estimated. The Kalman 
Filter design process results in relatively slow poles close 
to those of the plant being estimated. This is reasonable, 
as the filter was intended to minimize the mean estimate 
error when the plant and measurements are subject to white 
noise. The speed of these poles results in slow convergence 
to the actual states for initial errors in state estimates 
and disturbances caused by the untreated modes. 

Schmitz [VIII4] had limited success in dealing with 
this problem by using a higher order model, and generating 
fictitious estimates of the measurement noises. The 
fictitious noise estimates were parameterized, and the 
parameter space examined using root locus techniques to 
obtained a usable measurement gain, L, for the Kalman 


Filter. 


Use of the covariance values as a pole placement 
algorithm was not as direct or successful as the traditional 
method discussed in chapter 6 for the state observer design 
process. Use of the Kalman Filter design process to obtain 
pole placement is also thought to be impractical for future 
applications because: 

As measurements are added, as in this experimental 
work or in future multi-link systems, the parameter 
space becomes extremely large. 

• The model is not diagonal, and parameter variations 
are coupled, thereby making the search of the 
parameter space even more difficult. 

In view of what has been learned form this exercise, 
direct pole placement techniques, and traditional state 
observer design methods appear more suitable to the flexible 
arm problem at its current level of sophistication. 
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CHAPTER IX 

CHRONOLOGY AND ADDITIONAL EXPERIMENTAL OBSERVATIONS 

The work contained in the body of this dissertation 
chapters 1-8 follows a logical, cohesive path that is not 
completely representative of the course of work required to 
accomplish these results. This chapter discusses the 
sequence in which the experiments progressed, and additional 
interesting experimental observations regarding software, 
and hardware impacts on the experiments. The narrative and 
observations would interupt the flow of earlier chapters 
without adding insight to the results of a particular 
section yet they may be of interest and assistance to 
future researchers. 


9 . 1 Experimental Chronology 

Once the experimental apparatus had been assembled and 
verified a winding and broken path through the work 
described in this thesis began. This section tries to 
chronicle the sequence of events and comment on matters of 
possible interest to future investigation which might 
otherwise go unrecorded. 

Initial experiments focused on implementation of a 
functional observer instead of the observer/regulator 
combination and the Kalman Filter discussed in earlier 


chapters. A functional observer estimates a function of the 
states, for example a control law, instead of individual 
states. Functional observation can result in significant 
computational savings over state estimation, and then 
computation of the control. The computational savings 
associated with the functional observer was the source of 
the early attraction. 

Unfortunately, implementation of the functional 
observer yielded unsatisfactory results and dramatically 
demonstrated the negative aspects of functional observation 
when applied to a system with many uncertainties. In trying 
to analyze the source of the discrepancies between 
prediction and observation, the functional observer was 
proved intractable experimentally. 

The thesis effort took a dramatic turn at this point, 
trying to identify clear, functional divisions in the 
experimental apparatus and control System. Additionally, 
questions arose concerning the model and methods for it's 
evaluation. This led to functionally segmenting the system 
into a reconstruction algorithm, state observation 
algorithm, and controller as distinct elements. 

Following analysis and design of the individual 
functional elements, the experimental process was initiated 
again with the observer /optimal regulator combination. Some 
useful results were obtained, but stability and performance 
observations were far from expectation. This led to the 




sequence followed in the rest of the thesis, closely aligned 
with the chapter titles; 

1. Examination of the Model 

2. Examination of the Reconstruction Algorithm 

3 . Examination of the State Observation 

4. Examination of the Optimal Regulator 

5. Examination of the Kalman Filter 

This evolved into an iterative process of analysis, 
experiment, and comparison of the two. The process was 
repeated until confidence was gained that hardware, design 
software, and controller software bugs had been eliminated. 
The work reported in the body of thesis could then be 
conducted . 

Often the hardware, and software problems were not 
easily examined with the state space structure utilized for 
selecting control laws. As will be discussed in the next 
section, transfer function analysis was used in assessing 
the impact of many of the components. 


As the work progressed, usually meaning the 
implementation of higher gains in a specific control design, 
more faults or problems became apparent. The most 
aggravating and recurrent problems came from sixty hertz AC 
power noise picked up by the measurements, measurement phase 
lag introduced by the Butterworth filters, and short life of 
the potentiometer. The final limitation to evaluation the 
proposed control schemes with this experimental system 
appears to be computational speed. 


Sixty hertz noise repeatedly halted progress. It 
appeared as a distinct harmonic disturbance which at times 
excited modes in the 40Hz and 80Hz ranges. The sixty hertz AC 
noise was removed in phases by carefully assuring that the 
commons for all the signal conditioning devices were the 
same. Instrumentation methods of grounding and shielding the 
signal leads were implemented. 

Instabilities which were not predicted by the analysis 
techniques in use at earlier phases of the research were 
particularly hard to resolve. Analysis discussed in the next 
section using transfer functions identified the Butterworth 
filters as the culprit. The filters were implemented 
initially to act as band pass and band reject devices in the 
hopes of improving separation between modes. Eventually, 
after many unsuccessful attempts to compromise between the 
performance and phase introduction, the filters were 
removed . 

The mounting of the potentiometer used to measure 
joint angle rotations was a particularly troublesome 
component. A rubber grommet interfaced the potentiometer 
shaft to the motor hub. The grommet would wear and loosen, 
allowing slippage between the motor and potentiometer. The 
relative slip was of extremely small amplitude, and not 
noticed from observation of the endpoint position. 

The typical onset of the slipping condition was 
noticed by a reduction in stability of the controller under 
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observation. The gradual onset of the slipping condition, 
coupled with the fact that the work was almost always 
progressing towards a tighter control law or new 
implementation, made the identification of this problem 
extremely difficult for the first few occurrences of this 
malady. The remedy was to replace the grommet. 

9.2 Phase Sensitivity 

The instabilities introduced by the four pole 
Butterworth filters were not easily identified by the state 
space structure used for control law design. At the time the 
instabilities were occurring, the source of the difficulties 
was unknown. The model and controller were suspect, as well 
as the hardware components. To investigate the problem, a 
simple and thoroughly analyzed collocated controller [1X1-3] 
was implemented. This would hopefully allow for separation 
of the hardware issues from the modeling issues. 

Perhaps one of the most useful pieces of information 
available for resolution of the problem came from companion 
results for the unstable case with a passively damped beam. 
The passively damped beam showed an increased degree 
stability, and resultant analysis could be tested against 
the two cases. Figure 9-1 shows a time response for the 
experimental beam with collocated gains resulting in 
unpredicted instabilities. Surprisingly the instabilities 
are occurring in the second flexible mode. The measurement 
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is repeated with a passively damped beam, and this time the 
same gains do not result in instabilities in the second 
flexible mode as depicted in figure 9-2. 

Many components of the system were considered as 
sources of unmodeled, and hence uncompensated dynamics. 
These included sample rate (partially analyzed by sample and 
hold discretization) , digitization,, torque motor/amplifier 
dynamics, Butterworth filters, and tachometer. These factors 
were examined individually and cumulatively for their effect 
on system performance. 

These components as well as the beam were modeled as 
transfer functions, and assembled into an open loop transfer 
function, as shown in figure 9-3a, and a collocated feedback 
function as seen in figure 9-3. The transfer function, T(s) 
of Figure 9-3b, is the open loop function of 9-3a. The 
transfer function of the beam was common to the work of 
several researchers [1X4,5]. 

The collocated controller was also implemented in 
analog hardware to eliminate the questions regarding 
digitizing. 
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The various transfer functions utilized in the analysis are 
identified in table 9-1. 


Table 9-1. Transfer Functions 

A(s) Amplifier Transfer Function, 

Voltage In to Voltage Out 

E(s) Motor Electrical Transfer Function, 

Voltage Applied to Current 

V(s) Beam Transfer Function, 

Hub Torque to Joint Velocity 

K Motor Back Emf Constant 

e 

K t Motor Torque Constant 

b Amplifier Gain 

V d Desired Torque 

R_ Current Sense Resistor 

s 

K v Velocity Feedback Transfer Function 

K Position Feedback Gain 

P 

The resultant closed loop transfer function from 
commanded joint angle to joint angle could then be examined 
for varying gains by monitoring the closed loop pole 
locations. Evaluation of the torque motor/amplifier transfer 
function did not change the expected stable result for 
collocated feedback shown in figure 9-4. 

Analysis of the discretization effects by a sample and 
hold model did have a slightly destabilizing effect, but did 
not significantly alter the earlier result for the torque 
motor amplifier combination, even when examined together. 



Inclusion of a four pole low pass Butterworth filter 
used to filter commutation noise generated by the tachometer 
did have a drastic effect on the stability results. The 
drastic change in the departure angle from the open loop 
poles is depicted in figure 9-5. The analysis is repeated 
approximating the effect of the passive damping applied to 
the beam with modal damping values determined from frequency 
responses of the open loop beam. The plot is also depicted 
in figure 9-5. 

It is apparent from figure 9-5 that the second mode 
goes unstable at sufficiently large gains. Additionally the 
frequency at crossover closely approximates the frequency of 
the instability determined from measurements shown in figure 
9-1. This provides some confidence that the transfer 
function analysis employed is accurately modeling the 
observed phenomena. 

The dramatic effect of the filter poles being driven 
into the right half plane, even though they are at a 
frequency roughly 30 times the modal frequency, emphasizes 
the proximity of the flexible poles to the real axis. 





CHAPTER X 


SUMMARY AND RECOMMENDATIONS 
10.1 Summary 

This section briefly summarizes the important 
observations and results of the various sections by topic. 

• Linearized Model - The linear modeling technique 
was implemented based entirely upon manipulator design 
parameters. The responses predicted agreed well with 
measurements when coupled with appropriate selection of 
modal candidates and model order. The linear modeling 
technique was readily adapted for computer generation of 
models with different modal candidates and orders. The model 
predicted increased responses of the higher modes than was 
experimentally observed. The increased response predicted by 
the model is attributed to inaccurate determination of the 
damping for the higher modes. 

• Mode Selection - The application of feedback 
control laws to the flexible manipulator strongly impacts 
the resultant flexible vibration modes. It is extremely 
important to consider the control action in selecting modal 
candidates. The flexible manipulator's beam exhibits pinned- 
mass modes without control action, and as seen in chapters 
3, 7, and 8 may exhibit clamped-mass , or pinned-mass modes 
under the action of control laws. Clamped-mass modes were 



used in the LQR control implementation. 

• Model Order - The response of the flexible arm was 
dominated by the rigid and first flexible modes. The 
application of large feedback gains and the effects of 
discretization had a significant impact on the required 
model order. The faster the cycle time, the less the 
response of the higher modes. A change in cycle time from 
5.8 milliseconds, to 2.0 milliseconds resulted in almost 


complete elimination of 

the 

third 

flexible 

mode from the 

strain traces recorded 

for 

step 

responses 

obtained with 

collocated controllers. 

The 

speed 

was even 

more important 


than the inclusion of the second mode in the LQR controller. 

• Model Order Reduction - The application of the 
method of balanced realizations to the evaluation of model 
order agreed well with the qualitative evaluation made in 
chapter 3. Aggregation of the seven highest flexible modes 
of the ten mode model, (an aggregation level of six), 
resulted in only a 6.7% increase the performance index used 
to estimate model order requirements. The inclusion of the 
first two flexible modes and the rigid body mode brings the 
performance index to its maximum value of 100%. The method 
is based upon examining the strengths of the singular values 
of the linear model being evaluated. Therefore, in this 
application the method cannot evaluate the accuracy of the 
model in describing the physical problem. The method does 
provide a satisfactory estimate of appropriate order for a 
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selected model. 

• Measurement and Reconstruction - This technique was 
particularly successful when utilized for the single mode 
model. The evaluation for the two mode model is less certain 
due to experimental limitations. Measurement of strain due 
to the vibrations of the higher flexible modes truncated 
from the model was a particular problem. Measurement of the 
higher modes definitely contributed to the control actions 
exciting the third clamped-mass mode at 41Hz when large 
gains were applied. 

• Reduced Order Observation - The modal observers 
were successfully applied to the flexible manipulator. The 
most significant factor in the design process was to insure 
that the estimator was fast enough to accurately track the 
flexible mode amidst initial offsets and measurements of the 
higher modes. Slow observers, while successfully providing 
an estimate of the velocity, tended to excite the higher 
modes truncated from the model when used in feedback control 
laws . 

• Linear Quadratic Regulators - The application of 
LQR controllers to the control of vibratory modes occurring 
in flexible manipulators was successful in damping the 
vibratory modes of the system, as well as control the rigid 
orientation. The modal controllers were successful even in 
the presence of measurement noise and hardware 
imperfections, such as joint friction. 


The joint friction makes collocated controllers 
ineffective for precise control of the manipulator. Once the 
joint is close to the desired final state the applied 
torques become smaller than the torque required to overcome 
friction. At this point the system becomes uncontrollable 
for the collocated controller. Additionally, the beam 
vibrates in cantilevered modes without joint rotation and 
the vibratory modes are unobservable by the joint 
measurements . 

The fastest settling times were achieved with the 
single flexible mode controller designed with high modal 
damping. The settling times achieved with this design were 
on the order of one second for step changes in the desired 
angle of .35 radians. This settling time corresponds to 
roughly twice the period of oscillation of the first 
clamped-mass flexible mode. The quickest closed loop poles 
indicated by the model for the single mode LQR controller 
with a high prescribed degree of stability were -7.5 +/- 
.005i and -7.5 +/-19.13i radians/sec. If each of these pole 
pairs were considered as separate second order systems they 
would both correspond to a settling time, (to within 2% of 
the final states), of .533 sec.: The prescribed degree of 
stability design technique yielded poles with the same 
negative real parts. The response for the total system with 
these eigenvalues, settling times of 1.2-1. 3 sec., was 
roughly twice the the time for the individual poles. 
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Experimental attempts to shorten the settling time by- 
increasing the prescribed degree of stability in the control 
law design yielded instabilities in the flexible modes 
truncated from the model. 

• Kalman Filter - The application of Kalman Filters 
is not recommended for design of estimators to be used in 
controlling flexible manipulators. Estimates of the noise 
covariances yielded expectation of noise signals in the 
strain traces 40-50 Db below the strains caused by the 
neglected modes. 

The Kalman Filter designs yielded estimator poles 
close to those of the plant poles being estimated, much 
slower than indicated by the earlier results on modal 
velocity observation. Placement of the estimator poles by 
creating "fictitious" noise estimates and input distribution 
matrices was difficult and less direct than traditional 
methods . 


10.2 Discussion 

The most appropriate work for comparison is that 
conducted by Schmitz [XI] at the Guidance and Control 
Laboratory associated with Stanford University. This section 
will compare the experimental equipment, and results for the 
two efforts. 


Schmitz experimentally examined endpoint position 
control of a flexible manipulator structure. The endpoint 


position of the experimental manipulator was sensed by an 
optical sensor mounted external to the manipulator which 
viewed the intended objective for the endpoint motion. 

Schmitz employed a manipulator constructed with 
sidewalls made from aluminum sheets joined at regular 
intervals along the length by braces. The construction 
employed by Schmitz is similar to the light weight truss 
structures favored by aerospace engineers [X2-3]. The first 
three cantilevered modes of the manipulator structure were 
0.554, 2.781, 7.468Hz. Schmitz experimentally determined 
damping ratios for these modes in the range of .015-. 020. 
The manipulator structure was complex, and difficult to 
model. Schmitz assumed a decoupled modal form for the system 
dynamics and then executed identification algorithms 
experimentally . 

The aim of the work presented in this thesis was to 
construct a modal controller which determined and controlled 
the vibratory modes of the system, as well as the 
orientation in space. The objective of the sensor system 
implemented in in this study was to not require apriori 
knowledge of the final configuration of the manipulator. The 
controller and sensor system can be active over the full 
range of motions insuring control over the quality of motion 
and peak stresses. 

The continuous beam used for this thesis is 
representative of lightweight manipulators receiving 


attention for industrial [X4] and aerospace [X5] 
applications. The first three cantilevered modes of the beam 
are 2.08, 13.92, 41.38Hz with measured damping ratios in the 
range of .007-. 015. 

Schmitz achieved better agreement between his model 
and experiment, than was obtained in this work. This is not 
suprising as Schmitz used a higher order model, (three 
flexible modes and a rigid mode), and experimentally 
identified all the parameters of the system. 

Schmitz obtained endpoint position step responses with 
settling times of one second roughly half the period of 
oscillation of the first cantilevered mode. This compares 
favorably to the rough guideline established by Book [X6]. 
Book limits the upper response of a collocated controller to 
half the first cantilevered mode. The modal controller in 
this work obtained modeled settling times of .533 for each 
state, roughly the period of oscillation of the clamped-mass 
mode. The controller which tried to regulate both states 
with a single control input was half as fast. 

Part of Schmitz success in obtaining quick responses 
is attributed to use of endpoint position as the primary 
control. Additionally, the computation speed required by 
Schmitz is roughly one third that required in this work 
based on the ratio of flexible mode frequencies. The 
additional damping found in Schmitz's structure increases 
the stability of the system, as was found with the passive 
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damping treatment discussed in chapter nine. 

Schmitz considered that the upper time response limit 
attainable with endpoint control was caused by non-minimum 
phase zeroes associated with a time delay observed in his 
experiments from when torque is applied until tip motion 
occurs, (roughly 120 milliseconds for his apparatus). The 
modal controller developed in this work obtained strain 
measurements from the base of the beam and observed strain 
responses were effectively instantaneous with respect to the 
applied torques. The primary limitation encountered in this 
work was computational speed which limited model order. 

These two works are not contradictory, or mutually 
exclusive, and in most phases they are complimentary. The 
observed trends in stability and difficulties of obtaining a 
good dynamic model in the two efforts were in complete 
accord. The selection of approach for future work might well 
be driven by the chosen application, or even hybrid schemes 
investigated, (the possibilities for hybridization was also 
recognized by Schmitz) . 

10.3 Recommendations 

It appears clear at this point that the selection of 
the flexible vibratory modes is extremely important in 
generation of a useful model. Additionally, identification 
of the manipulator structure is not the answer, as the 
application of feedback control laws alter the modes 


observed in closed loop form of the system. It seems 
appropriate, then, that methods be sought which incorporate 
the feedback law into the modeling process, (Majette [X7] 
considered this problem for manipulators with small motions 
about an equilibrium). These methods are thought to come 
from iterative processes between beam solution techniques 
and Lagrangian dynamics. 

Future flexible manipulators consisting of serial 
links and complex geometrical configurations will require 
the inclusion of many more modes in the model. These modes 
arise from torsion and bending in more than one plane for 
each link. Models including modal series consisting of a 
large number of terms may never be realizable in available 
real-time controllers. Based on this experimental history it 
is doubtful that blind truncation of model order will yield 
successful results when high performance of the manipulator 
is required. 

Methods need to be adapted which account for the 
higher mode's responses even when designing for a lower 
order controller. Frequency domain methods [X8] promise to 
accommodate the feedback boundary conditions in the model as 
well as robust controller design. The research issues facing 
the frequency domain approaches lie in merging the physical 
measurements with the frequency domain description and 
computational requirements of practical implementations. 



Balas [X9] proposed phase-locked loop filtering for 
the rejection of the untreated higher modes from the control 
or the measurement. Filters which remove information about 
the truncated modes from the measurement is still an 
appealing technique for research, although the attempt in 
this work with analog butterworth filters, as discussed in 
chapter 10, was. disappointing. 

Design of structures which inherently reduce the 
response of the higher modes should be pursued. This type of 
structure would greatly reduce the control problems 
encountered in this work. Combining attempts at designing 
"lossy" joints, and beams, such as passive damping [X3] 
treatments, may result in successful high performance hybrid 
active/passive schemes . 
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APPENDIX A 

FORMULATION OF DYNAMIC EQUATIONS 

This section describes the generation of a dynamic 
model via application of Lagrange's equations to the 
flexible system [Al,A2]. 


A» 1 Coordinates 

The first step in this process is to select a suitable 
set of coordinates. The approach utilized selects one rigid 
body coordinate associated with the joint rotation, and 
flexible transverse displacements from a set of axes 
attached to the joint. This is depicted in figure A-l. Then 



Figure A-l. Coordinate Definition 
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a position vector R to every point of the system can be can 
be constructed; 

R = xi + w(x,t)j ( A. 1 ) 

The absolute velocity of the position vector; 

R = 3R + §kxR (A. 2a) 

3t 

R = xi + 3w(x,t)j + 9xj-0w(x,t)i] (A. 2b) 

at 


A. 2 Kinetic and Potential Energies 
The kinetic energy of the system KE, can then be 
computed by integrating this expression over the entire mass 
of the flexible system M ; 

S 


KE = 


1 

2 


R*Rdm 


(A. 3a) 


R*R = [aw(x,t)] 2 + 293w(x, t)x +0 2 [x 2 + w 2 (x,t)] (A. 3b) 

at at 


KE 



{[3w(x,t)] 2 + 29jiw(x,t)x + 

at at 


(A. 3c) 


9 2 [ x 2 + w 2 (x,t) ]}dx 


Next, introduce the assumed mode series representation for 
the transverse deflections w(x,t); 

w(x,t)=£ <j> i (x)q i (t) 




for i=l , 2 . . .n 


(A. 4) 


where n = x/L a normalized length variable. Substitution for 
the transverse deflections results in; 


KE= 2 


V<J). (ii)dg. (t)Y<fr .(ri)dg. (t) 
L dt 1 L 3 at 3 


(A. 5a) 


2en^<t> 1 (n)dq i (t) + e 2 [(Ln) 2 + ^ i (n )q i ( t)p>j (x)q_j ( t) ] }dm 

This integral can be separated into three integrals over the 
primary components of the beam, joint mass, beam mass, and 
payload. Evaluation of equation A. 5a over the joint mass 
results in; 

KE = 5 (0) ^2i (t)Ydi.(0)dg.(t> + 0 2 ] (A. 5b) 

m 4 u ^dn dt 1 ^dn 3 dt : 


Evaluation of equation (A. 5a) over the mass 
results in; 

^ir 5V 2 iV L 


y«j>. ( ti ) dq- ( t))<fr • (n)d2^ (t) 
L 1 dt 1 L 3 dt 3 


of the beam 


+ (A. 5c) 


20r)Ly ^ (r| )dq^ ( t ) }dri 
^ dt x 


Notice that in evaluating; 

[0 2 ri 2 + ^iJ> i (Ti)q i (t)^(j) j ( T| )q^ (t) ]dn (A. 6) 


K pL 


the squared flexible deflections was assumed negligible 
compared to the axial dimension squared. This linearization 
step can be postponed to until the equations of motion are 
formed, but this results in applying the same assumption to 
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multiple terms. Finally the integral is evaluated over the 
mass of the payload as; 

KE = s[H L 2 +J ] 6 2 + hi V* . (l)dg. (t)Y<j> .(l)dg. (t) + (A. 7) 

p * p p ^ PL A dt 1 1* J cit^ 

i/ 2J n M Vd^. (n)dgi (t)Vdi. (riJdg. (t) 

p PL dn dt 1 ^dt- 1 dt- 1 


Next, it is convenient to introduce an orthonormal condition 
on the spatial mode functions. 


K pl 


(k (ri)dg. (t)V<j) . (ri)dg. 

L 1 dt i L J dt- 


(t) + 


(A. 8) 


i M p[* i <D|| i (t,^j(l)|2j(t) + 

= 1- for i=j 

z p ^dt 1 dt 1 ^dn- 1 dt J 0, for i=j 


The potential energy, PE, for the system is evaluated by the 
following integral expression; 


PE = 


l EI 


[ Y d 2 <t ^ ( ri ) cr^ ( t ) Y d 2 <j> j (Ti)Qj (t) ]dx (A. 9) 

dx 2 dx 2 


Applying the orthogonality condition on the mode functions, 
and substituting the normalized length variable yields; 


PE = 


hlfkdV (r, ) ] 2 q. ( t) }dn 

l 3 J l d , 2 


(A. 10) 


Notation in the following sections can be greatly simplified 
if the following definitions are made for a "modal 
stiffness", , for equation (A. 10), and "moment of modal 
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mass", W^, for the last integrand in equation (A. 5b) as; 


K i = pi 

V 


T [d l (n)] 2 dn 
L 2 X 
dri 


(A. 11) 


W i = 


nL|<ti i (Ti)<iTi 


(A. 12) 


The kinetic energy for the system can be expressed as; 


KE 


= 3® 2[J 0 +J p +M p l2] + ^y^2 i < t )[W i +LlC*(l) + (A. 13) 


^dt 


L J <t» ( 1) ] + 


iTctei 

dt 1 


(t)] 


A. 3 Lagrange's Equations 

To generate the dynamic equations the Lagrangian of 
the energy expressions are formed, where the q^ are the 
coordinates, and Q i represents the work done by the input 
torque at the joint by each coordinate. The result^pt 
equations can then be organized in matrix form; 


d 

dt 


3KE 

H, 


a pe 

Hi 


= Qi 


[M3 Z + [K]Z = [Q] 


(A. 14) 
(A. 15) 


' q n (t)] 


Z = [0,q 1 (t) ,q 2 (t) 


(A. 16) 
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M = 

J ♦ J ♦ J ♦NL 2 

0 b p p 

*1 * L Vl (1) * 

dn 

*2 * ♦ V*2 (1 > 

dn 


(A. 17) 


V “W” ♦ - 2 ♦ fyti) 


K = 


Q = 


1 


0 K x 0 


0 0 k 2 

• • • 

• • • 

• • • 


dfci (0) 

dn 1 

di,(0) 

dn^ 


(A. 18) 
(A. 19) 


This system is easily organized into a linear state-space 
model as shown in figure A- 2. 
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Figure A-2. State Space Representation. 



APPENDIX B 


BERNOULLI -EULER BEAM EQUATIONS 

This section describes the development of a frequency 
determinant from Bernoulli -Euler beam theory which was used 
to derive candidate mode frequencies and the associated 
shapes. The homogeneous differential equation is presented 
first, followed by a discussion of the boundary conditions 
utilized. Lastly the frequency determinant is derived. 

B.l Differential Equation 

The transverse displacement of the beam, w(x,t), shown 
in figure A-l is a function of both the spatial variable 
along the beam, and time. Following the analysis attributed 
to Bernoulli and Euler gives rise to following fourth order 
partial differential equation. 

EI3 4 w(n,t) - A. L 4 3 2 w(n,t) =0 (B.l) 

4 D 2 

3ri 3u 

where: n=x/L The next step applies the separability of 

equation (1) to obtain the following result; 

EId 4 <j> ( n )q( t ) — p A-L 4 <J) ( n )dq( t ) = 0 (B.2) 

4 ° 2 

dn dt z 

Searching for periodic time functions of the form q(t)= e 1U)t 
leads to the following formulation; 
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# 


EI[d£ 4 (Ti) -pL 4 A, ct!(n)(jJ 2 ]q( t) = 0 (B.3a) 

dn 

This implies that the term in brackets must be equal to zero 
for all time t. This is expressed as: 

d£ 4 (n) - f3 4 <Mt) = 0 (B. 3b) 

di) 

where the new parameter, 

6 4 = pL 4 A b <u 2 ( B . 4 ) 

has been substituted. This is readily solved for <J>(ri): 

<j> (n )=Asin( pn)+Bcos( 3n)+Csinh( gn )+Dcosh( gn ) (B. 5 ) 

The solution for the spatial mode function <J> ( ri ) requires 
four independent boundary conditions be provided. The first 
and most obvious results from noting that there cannot be 
transverse displacement at the pinned joint, this takes the 
form; 

<Mr|) = 0 , for ri = 0 (B.6) 

The second condition is provided from a moment balance at 
joint, this is expressed as; 

d 2 <fr ( n ) = -J Q g 4 dj) (n) for n = 0 (B.7) 

dr| 2 pA^L^dri 

where the following substitution was made to eliminate the 
dependence on the frequency; 

a 2 = B 4 EI 
pA b L<l 


( B . 8 ) 



Using this boundary condition results in pinned mode 
shapes for small joint inertias. Additionally, clamped mode 
shapes can be determined by inputing very large joint 
inertias. This provides more programming versatility than 
supplying one formulation of the frequency determinant for 
each type of boundary condition. The third boundary 
condition is derived by resolving the shear force at the end 
of the beam against the inertial forces of the payload mass. 
This takes the following form; 

d 3 <j> ( n ) = -M pB 4 d> ( t ) , for n = 1 (B.9) 

dn 3 pA b L 3 

The last boundary condition arises from a moment balance 
against the angular inertial forces of the payload. 

d 3 (j> (n ) = -J n B 4 d<i) (n ) for n = 1 (B.10) 

2 u 3 

dTi pA^L ^ 


B . 2 Frequency Determinant 

Application of the boundary conditions to the solution 
for <{> ( r| ) will result in a frequency determinant for the 
eigenvalues p. Application of the first boundary condition 
for transverse displacement at the joint relates two of the 
constants in the solution; 

B = -D (B.ll) 

The second boundary condition balancing the moment at 
the joint forms a relation between three of the of the 
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4 



constants; 


2DJ n =-pA. L 3 g 3 (A+C) 


(B . 12 ) 


The shear force balance at the payload relates all the 
constants of the solution; 


A(M_gsing-cosg ) + B( sing+M gcosg ) + 


( B . 13 ) 


A. L 


A, L 


C(M fisinhg+coshg ) + D( sinhg+M Bcoshg ) = 0 


A b L - 


A, L 


The moment balance at the payload forms a similar relation; 

( B . 14 ) 


3 3 

-A(smg+J g cos g) + B(J g sin g-cosg) + 


V 




3 3 

C(smhg-J g cosh g) + D ( - J g sinh g+coshg ) = 0 


A b L' 


A b L ' 


The expressions (B.ll- B.14) involve only the constants from 
the solution for the mode function and the parameter g . This 
can be configured in matrix form as; 

A 


F(g) 


= 0 


Fll=sing+sinhg + J *g (cosg-coshg) 


F12 = sing-sinhg + 2coshg +fl *g (cosg-coshg ) + 


( B . 15 ) 

( B . 16 ) 
( B . 17 ) 


Jn*r 


2 sinh g 

J o*^ 3 


F21=cosg+coshg - M*g( sing-sinhg) 


( B . 18 ) 



F22 = -cos$ - coshp + 2sinh3 + J_ „.8^ ( sinhB-sinB ) - (B.19) 

3 p 

J o* p 

2J_*cosh8 
P* 3 

J o*e 

Where the starred subscripts indicate modification by the 
appropriate area and length terms. The roots of the 
frequency determinant det[F(8 i )] = 0 yield the 

characteristic values for the mode functions <j>^(n)/ and 
associated frequencies 


APPENDIX C 


LINEAR QUADRATIC REGULATOR 

This appendix discusses the design and implementation 
of deterministic Linear Quadratic Regulators ( LQR ) . The 
first section formulates the governing equations for the 
continuous case and applies the sweep method to obtain the 
Ricatti equation. This presentation is a compilation of the 
material contained in Bryson and Ho ' s text [ Cl ] on optimal 
control. The section discussing controller design with a 
prescribed degree of stability follows Moore [C2]. The last 
section considers the necessary adaptations required for the 
experimental sampled data system. 

C ♦ 1 Continuous System 

The earlier sections discussed the development of the 
state-space equations for the flexible manipulator system. 
This system can be represented by the general expression; 

x = f(x,u,t) (C.l) 

The model for the flexible manipulator does not require 
explicit expression of the time variable and f(x,u,t) has 
the following linear form; 
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The goal of the process is to determine a gain vector 
T 

, K , which determines the input function, u(t), as a linear 

combination of the states, x(t), in an optimal manner. The 
T 

gain vector, K , will be "optimal" in the sense of a 
specific performance measure. The measures of performance 
are called Performance Indices (PI), and for LQR controllers 
the PI is defined in the following manner; 


PI 


1 

2 


[x T Qx + u T Ru]dt 


( C . 3 ) 


Q and R represent weighting matrices which selectively 
penalize the various states, x, and input, u, during the 
process. The next step is to adjoin the system dynamic 
equations (C.2) to the performance index by the use of the 
time varying functions X(t); 


PI *5 


{x T Qx + u T Ru + X T [f - x]}dt 


( C . 4 ) 


Integrating the adjoined performance index by parts yields; 

t. 


T 

PI = AX 




[x T Qx + u T Ru + X T f + X T x]dt 


( C . 5 ) 


Notation can be simplified by defining a "Hamiltonian" [Cl] 
function H(x,u) for the system as; 

H(x,u) = x T Qx + u T Ru + X T f ( C . 6 ) 

Substitution into equation (C.5) yield the more compact 
expression; 
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f 

PI = X T x + | [ H ( x , u ) + X T x]dt ( C . 7 ) 

To find the minimizing functions, variational principles are 
applied to the performance index. Small perturbations about 
the optimal solution are considered; 

*"f 

6 PI = A T 6x + \ {[6H(x,u) + A T ]6x + 5H6u}dt (C.8) 

. 9x 3x 

r 0 

The optimal solution x(t), u(t) is an extremum for the PI 
and therefore the variations must be zero. The task is now 
to find the functions x(t), u(t) which satisfy this 
condition. 

C.2 Modification for Solution 
Solving the set of equations (C.8) can be simplified 
by noting that for a regulator the initial and final states 
are known, and choosing the multiplier functions u ; 

<5x = 0 , for t=tQ, and t=t^ (C.9) 

X T = 3H (C.10) 

3x 

The variation in the PI can now be expressed as; 

<5PI = \ 6H6udt (C.ll) 

^J5u 

The variation in the performance index must be zero for 
arbitrary variations in the control function u. This can 
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only be true for; 


5H = 0 
3u 


( C . 12 ) 


Equations (C. 9, 10, 12) are known as "Euler-Lagrange 
Equations" [Cl] for the system. 


C.3 Sweep Method 

The "sweep method" will be presented in the next 
section as one approach to obtain the Ricatti equation. 
First a solution, S(t), relating the states, x(t), to the 
multiplier functions, X(t), with the following form is 
assumed; 


T 

X 1 = Sx 


Substitution into equation (C.10) yields; 

9 ( Sx ) = -CQX + X T A ) 

at c 


( C . 13 ) 


( C. 14a) 


or carrying out the indicated partial differentiation, and 
substituting equation (C.13) for X; 


Sx + Sx = -(Qx + A„ Sx) 

c 


( C. 14b) 


Next, the differentiation indicated in equation (C.12) is 
carried out, and the resulting expression examined in more 
detail; 


T T 

3H = 0 = u R + X B 
3u C 


( C . 15 ) 


This can be formulated for the input function u(t) as; 
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-1 T 

U = -R n 1 A 


( C . 16 ) 


This result can be substituted into equation (C.2) to 
obtain; 


X = A X - BR^ - § Sx 
c 


( C . 17 ) 


Equations (C.14,17) can be combined to form the "Matrix 
Ricatti Equation" for the system. 


[S + SA - SBR" 1 B T S + Q + A_ T S ] x= 0 
c c 


(C. 18a) 


This expression must hold true for arbitrary x, therefore 
the expression inside the brackets must be zero; 


S + SA - SBR“ 1 B T S + Q + A„ T S = 0 
c c 


(C. 18b) 


The "sweep method" is so named because the usual 
solution technique for this problem is to sweep backward in 
time from the final condition to the start. Regulators are 
designed by finding the steady state solution, dS/dt equals 
zero, by finding the solution matrix S of; 


SA - SBR _1 B T S + Q + A„ T S = 0 
c c 


( C . 19 ) 


C.4 Prescibed Degree of Stability 
In this section modification of the LQR regulator 
problem to include specification of minimum stabitility in 
the design process. The objective of this modification is to 
design the optimal control law in such a was that the closed 
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loop eigenvalues have negative real parts less than a value 
(-a). This technique was first studied by Anderson and 
Moore [C2], and discussed for application to flexible 
manipulators by Sangveriphusiri [ C3 ] . 

The modification starts by considering the following 
definitions. 

x' (t)= e at x ( t ) ( C . 20 ) 

u’ (t)= e at u ( t ) ( C . 21 ) 

From these definitions, it is clear that x't) and 
u'(t) will be stable (i.e. x'(t) or u'(t) -+ 0 as t -► 00 ) only 

“ 3 1 

if x(t) and u(t) decay faster than e . This is equivalent 
to requiring the closed loop system to have a degree of 
stability of at least -a. 


Differention of equation (C.20) yields. 


x' = 


at 

a e x 


e at x 


( C . 22 ) 


Substituting equation the linearized form of the state 
space model, equation (C.2), for x(t) into equation (6.20), 
the modified system equation can be written as follows: 


x' = (A+aI)x' + B u' 


( C . 23 ) 


The function inside the integral sign of the cost 
function is modified to 

u' T R u ' + x' T Q x' =e 2at (u T R u+x T Q x) 




( C . 24 ) 
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Instead of solving the previous regulator problem, 
equation (C.2), one solves the modified system of equation 
(C.23) with the following modified performance index is 

(x' T Q x'+u' T R u' )dt ( C . 25 ) 

JO 

A similar form of solution will be expected. The 
control u' (t) is the linear function of state, i.e. 

u' =-Kx ' ( C . 26 ) 

and the matrix-valued function, K, can be evaluated from 

K =R~ 1 B T S ( C . 27 ) 

where S is the solution of the Riccati equation. 

Next, it can be shown that a feedback control law for 
the modified problem readily yields a feedback control law 
for the original problem. Substituting the definitions of 
equations (C.20) and (C.21) into equation (C.26), 

u =-e at u'=-e~ at K(e at x) =-K x (C.28) 

So that, the optimal feedback gain of the modified regulator 
problem can be selected as the control law of the original 
problem and the closed loop system of the original problem 
will have a degree of stability of at least -a. 

C.5 Sampled Data System 

This section discusses translating the continuous LQR 
design problem to the sampled data case which approximates 
the digital implementation employed for the experiments . 
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The approach taken will be transform the dynamic equations 
and the weighting matrices to the discrete domain and then 
reformulate the problem for solution. 

Inputs to the plant from the micro-processor 
controller are "held" constant between cycles of the 
controller. This type of sampled data control system has an 
equivalent discrete dynamic equation to the continuous plant 
dynamic equation (C.2) which is the solution for continuous 
equation over the time interval. The solution is given by; 

x i+1 = A^x^ + Bdu i (C.29a) 


where the subscript i indicates a particular sampled time, 
and i+1 indicates the next, and; 

A^ = exp(A c At) (C.29b) 


B d = 


exp( A t )B u.dx 
c ci 


( C. 29c) 


The goal of this process is to design a feedback control 
which determines the input sequence u^ as a function of the 
state vector sequence x^; 


u i = K d x i 


( C. 29d) 


The performance index expressed by equation (C.3) is 
transformed from an integral into a sequence of responses 
over the sampling interval At. 


PI 


-l 


{[exp(A c x)x i + ( 


exp(A c C)B c u i )d^] x Q 


( C . 30 ) 


C -~b 
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[exp(A c x)x i + ( 


exp( A c ?)B c u i )d5] ) + u i Ru i }dx 


This expression can be formulated with almost a direct 
correspondence to the terms of equation (C.3); 

PI = )(x. T Q,x, + x. T W,u. + u . T R jU ) (C.31a) 

L x & x x d i id 


where ; 


Q d 


exp(A c 5) x Qexp(A c C)dC 


W d = 2 


exp ( A c t ) Q[ 


R d = 


{R + [ 


exp(A c UB c u i )d^]Q[ 


( C. 31b) 


exp(A £)B u. )d£]dx (C.31c) 

C C X 


exp(A c 5)B c u i )d^] }dx 

( C. 31d) 


Without the cross terms associated with W d in Equation 
(C.31a) the sampled data representation would be directly 
analogous to equation (C.3). A pre-filter gain F can be 
chosen to eliminate the cross terms; 


F 


( C . 32 ) 


Another input sequence can be defined utilizing the pre- 
filter gain F as; 


v i = Fx i + u i 


( C . 33 ) 
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such that; 


x i+i * V + B d v i 


(C.34a) 


where ; 


A d = A d “ B d F 


( C. 34b) 


Making one last transformation for the state penalty matrix 
Qd; 


2d = 2d- Fd F 


( C . 35 ) 


yields the desired equivalent form of the performance index 
for the sampled data system. 

( C . 36 ) 


PI = E(x. T Q^x. + v i T R d v i ) 


The solution of this problem subject to the dynamics of 
equation (C.34a) will yield a solution vector which 
coupled with the pre- filter gain F yields the desired gain 
vector K d for the original sampled regulator; 


K d = K d + F 


( C . 37 ) 


The variational approach, and Sweep- Method presented 
for the continuous case carry through for the sampled data 
regulator by direct analogy using sequences instead of 
continuous functions. The process is extremely 
straightforward and will not be presented here. The 
resultant form of the steady state Ricatti equation for the 
sampled data regulator is; 



C.6 Ricatti Equation Solution 


The Newton-Raphson, and other methods have been 
formulated for this problem repeatedly, and software is 
readily available. Two implementations were utilized for 
this work to provide cross checking. Routines extracted from 
the ORACLS [ C4 ] , and Control-C[C5] software packages were 
executed on the flexible arm model. 


APPENDIX D 


KALMAN FILTER DEVELOPMENT 

This section discusses the development of a Linear 
Quadratic Gaussian Regulator ( LQGR ) this is more commonly 
referred to as a Kalman Filter [Dl-2]. The basic relations 
and solution technique will be discussed as was done in 
appendix C for the deterministic regulator. For brevity only 
the sampled data case will be developed here. 

D.l Governing Equations 

Consider the dynamic equations for the system now 
subject to the introduction of a zero mean gaussian noise 
to the plant and v^ to the measurement y^. 

X i+ 1 = Ax i + Bu i + w i (D.l) 

y i = Cx i + v i (D * 2) 

The noises are uncorrelated and have covariances given by; 

E[w.w. T ] = R (D. 3 ) 

l j w 

E^v.. 1 ] = R v (D. 4 ) 

The objective of this formulation is to select the 
measurement gains for the full state observer depicted in 
figure D-l in an optimal manner. 

Many notational definitions are required to form a 
tractable formulation of the Kalman Filter equations [D3]. 
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The definitions are summarized in table D-l for easy 
reference. 


Table D-l. Summary of Kalman Filter Notation 

Symbol Comment 

Plant state vector 
Estimate of the Plant state 
with measurement update 
Estimate of the Plant without 
measurement update 
Error between plant state and 
estimate e = (x - x) 

Error between plant state and 
estimate e = (x - x) 

Covariance of error e 
M = E[ee] 

Covariance of error e 
P = E[ee x ] 

♦ 




Figure D-l. Block Diagram for Kalman Filter 
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Consider the error e for an estimator constructed with 
actual plant dynamics; 


e t+i = x i+i - x i+i = A d ,x i • x i> + Vi (D - 5 > 

Then the covariance of the error without the measurement 
update i s > 

M i+i ■ “vaV + Vi w i Tl (D - 6a) 

Or in more compact notation; 


"w = va 1 * w/ (D - 6b) 

Consider a performance index for the system with the 
following form; 


PI = l/2I[e i T M“ 1 i e i + (y i -Cx i ) T R v -1 i (y i -Cx i ) ] (D.7) 

The task is again to find the sequences which minimize the 
performance index, therefore, we wish to find the conditions 
which will yield the sequences. Examining the effect of 
small arbitrary variations in the state vector x^; 

6PI = 6x i [M -1 i e i + C T Rv" 1 i (y i -Cx i ) ] (D.8) 

For the sequences to minimize the index the coefficient of 
the variation must vanish; 


M' 1 (x i -x i ) + C T R v " 1 i (y i -Cx i ) = 0 


(D. 9 ) 


This can be organized in terms of the plant state; 


-1 T -l -l ~ T -1 

(M i +C 1 R v i C)x i = M x L x + C x R v i i y i 


(D.10) 
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forming the the feedback grouping results in; 

(M' 1 1 +C T R v ' 1 i C)x 1 = (M" 1 i +C T R v " 1 i C)x i + (D.U) 

c T V 1( yi- Cx i) 

Dividing through by the coefficient of the state, x^, 
provides a relation between the estimate without update, x^, 
the measurement, and the state which minimizes the 
performance index. 

x i = x i + L i (Y i _Cx i ) (D. 12a) 

where a Kalman gain, L, has been introduced; 

L i = (M" 1 i +C T R v _1 i C)' 1 C T R v ~ 1 i (D. 12b) 

Thus an optimal estimate, x^, of the state can be formed 
using the measurement, y^, and the estimate without update, 
x^, which satisfies the same conditions; 

x i = x i + L i (y i -Cx i ) (D. 13 ) 

It is of interest to separate the terms forming the Kalman 
gain as; 

L i = P* i C T R v ' 1 i (D. 14a) 

where the grouping; 

P* i = (M" 1 i +C T R v _1 i C)' 1 (D. 14b) 

has been formed. It will now be shown that the grouping 
shown in equation (D.14b) is in fact the covariance matrix, 
P^. The error for the estimate with update, e, can be 


written as; 
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e i * x r x i = x i- x i + 
P* 1 C T R v ' 1 1 [v i -C(x i -x i )] 


(D. 15a ) 


Rearranging terras, and using the notation e^ for the error 
before measurement update; 


e i * 'I + p *l c V 1 i c,e i + p *i c X'Vi 


(D.15b) 


Then the expected values can be determined; 

P i = (I+P* i C T R v ‘ 1 i C)M i (I+P* i C T R v " 1 i C) T + L i R vi L i T (D. 16). 


Next, consider the inverse of equation (D.14b); 

* -1 -1 T -1 

p i * M V C X i c 


(D.17) 


Premultiplication of equation (D.17) by P*^, followed by 
postmultiplication with yields; 


* * T -l 

M i = p i + P i c R v i CM i 


( D. 18a) 


or; 


* T -1 * 

(I+P i C i R v i C)M i = P i 


( D. 18b) 


The result, equation (D.18b) can be substituted into 


equation (D.16) to eliminate the term; 

P i = P i (I+P i C X i C > + L i R viV 


(D.19) 


This can be regrouped using equation (D.14a) for the Kalman 
gain twice; 

P i = P* i (I+L i C) T + ( p * i C T R v " 1 i )R vi L i T (D. 20a) 


, finally after some simplification; 


* * T T * rp m 

P i = P ± - P i C x L i + P i C L i i 


(D. 20b) 
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This yields the needed result; 

P i = P* i ( D . 20c ) 

Now the Kalman gain, L^, can be expressed in terms of the 
covariance P^; 

L i = P i C T R v " 1 i (D. 21 ) 

This completes the derivation of necessary Kalman Filter 
equations. The next section will discuss a method for 
solution of the steady state gain gain L. 


D.2 Solution Method 

Iterative solutions using expressions for P^ and 
converge very slowly for discretizations of the plant for 
short time intervals. Formulation of the problem into a 
Ricatti equation suitable for the same Newton-Raphson 
algorithm employed for the deterministic regulator covered 
in appendix C is very desirable in terms of convergence 
properties and software usage. 

To achieve this goal, equation (D.15) is rewritten; 


e i+l = (A d' L i C)e + L i v i + B d w i 


(D. 22 ) 


The expected value can then be determined; 

P i+1 - <V t 'i C,P i ( V l i C,T + L i R v L i T + B d R w B d T (D - 23) 


Steady state is reached when P^ +1 is the same as P i , this 
gives the necessary form; 

P = (A d -L i CIP 1 (A d -L i C) T + + B^b/ 


(D. 24 ) 


Combining this with expression (D.21) for the Kalman gain 
completes the process. 


APPENDIX E 


EXPERIMENTAL APPARATUS 

This section of the thesis describes the the 

experimental manipulator system. Manufacturers, 

specifications, experimental measurements made to assure the 
performance parameters of key components, and detail 
diagrams identifying the actual electrical connections are 
presented. 

The experimental hardware is separated into five 
functional areas, and identification and description of the 
hardware components will proceed sequentially through the 
functional groups. Electrical schematics for components will 
be given presented in the functional groups , but 
interconnections for subsytems will conclude the appendix. 

The following list identifies the functional hardware 
groupings : 

• Flexible Manipulator 

• Sensors/Signal Conditioning 

• Analog to Digital Conversion 

• Micro-Processor System/Digital to Analog 
Conversion 

• Torque Motor/Servo-Amp 



The microprocessor is discussed in appendix F with the 
controller software. 

E.l Flexible Manipulator 

The first piece of hardware to be discussed is the 
actual flexible link, and payload. This component sets the 
scale for the experiment. The arm is a four foot aluminum 
beam with the section oriented so that axis of increased 
flexibility is in the horizontal plane. The manipulator 
mounted in its base with sensors is shown in figure E-l. 

The payload is provisioned for the addition of weights 
giving it a range of five to one in increments of one 
quarter of the base amount. Table E-l lists physical 
properties, and dimensions for the beam, and payload. 




Figure E-l. Manipulator with Sensors. 



187 


E.2 Sensors 

The manipulator and base are equipped with several 
sensors to obtain information about the state of the 
flexible system. This section discusses the individual 
measurement systems. 

E.2.1 Joint Angle Sensor 

Rotation of the joint is measured by the use of a 
rotational potentiometer connected to a high impedance 
signal amplifier. This is depicted in figure E-2. Ninety 
degrees of rotation is scaled to +/- 5 volts DC by use of the 
signal amplifier. 

E.2. 2 Joint Angle Velocit 


An Inland motor tachometer is utilized to measure the 
angular velocity of the joint. The tachometer is also 
connected to a signal amplifier, this provides analog signal 
scaling and isolates the tachometer from line loads, as well 
as reducing noise. 

Table E-l. Physical Properties of the Beam 

and Payload 


Beam 

Length = 48 in. 

Section: 3/16 x 3/4 in. 
Material : Aluminum 


El Product= 4120 
Alloy: 6065-T6 


Payload 

Material: Aluminum 
Diameter= 1.25 in. 


Alloy: 6065 -T6 
Thickness= .75 in. 



The manufacturers specifications for the tachometer 
are listed in table E-2. Measurements were made of the 
tachometer to verify the manufactures voltage/speed 
constant. The joint was rotated at a constant velocity for 
several revolutions, the output voltage measured, and the 
velocity computed. The data is presented in figure E-3. 
Computation of the slope provides a constant ,. 9volt- 
sec/radian, relatively close to the specified amount, 
1. Ovolt-sec/rad. 

E.2.3 Strain gages 

State feedback using modal variables obtained from 
reconstruction (chapter V), and estimation (chapter VI) is 
based on measurement of strain due to bending at the 
surface of the beam. Strain at a point on the surface of the 
beam has contributions from axial stress, torsional stress, 
and out of plane bending. Additionally, strain gages 
generate low level signals, and are sensitive to 
temperature . 

The specific implementation for this experiment 
consisted of a four active element bridge commonly used to 
measured planar bending of beams [El]. This configuration is 
much less sensitive to stresses due to torsion, extension, 
transverse bending and provides higher signal levels in 
bending than an individual gage. Temperature is also 
compensated. 


Figure E-4 depicts the mounting arrangement for the 
active gages in the bridge at a single measurement point. 

The performance of the bridges was examined by locking 
the joint and deflecting the endpoint in fixed increments. 
The result is shown figure E-5. This result was compared 
against linear elastic theory, allowing for calibration of 
the bridge, and amplifiers in one step. 

The gages are driven by a constant voltage bridge 
control circuit (Honeywell Accudata 105), and the resultant 
change in output is amplified (Honeywell Accudata 122) 
before being connected to the by the analog to digital 


converter . 
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Figure E-4. Strain Gage Implementation. 
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Figure E-5. Bridge/Amplifier Response. 


Hardware Identification 
Table E-2 


Device 

Function 

Manufacturer 

Specifications 

Potentiometer 

Joint Angle Sensor 

Bourns 

Part no. 658-S-1-502 

5 kohm, 1 turn, Cermet 
resistive element 

Amplifier 

Scaling/Isolation 
of Joint Angle Signal 

KEPCO 

Model no. BOP36-15M 

2.0amp, 36 volt 

Unity gain crossover 300khz 

Tachometer 

Joint Angle Velocity 
Sensor 

Inland Motor 
Model no. TG-2139A 

.9 volt-sec/rad 
max speed 77 rad/sec 

Amplifier 

Seal ing/Isolation 
Joint Velocity 

Hewlett Packard 
Model no. 6823A 

lamp, 20volt 

Strain Gage 

Dynamic Strain 
Measurement 

Mi cro-Measurement 
Systems 

Part no. EA-13-250-350W 

Two gages per element, 
250 ohms, solder tabs 

Bridge 

Drive Strain 
Gage Bridge 

Honeywell 

Model no. Accudata 105 

Four active element, 5-10 
volts DC excitation 

Amplifier 

Scaling Strain 
Signal 

Honeywell 

Model no. Accudata 122 

Variable gain, 
.02-5000 

Motor 

Joint Torque 

Inland Motor 
Model no. T-5730-M 

Peak torque = 85 in-lbs, 
peak current= 15 amps 

Power Amp 

Motor Current 

KEPCO 

Part no. B0P15-20M 

20amp, 15 volt 
300khz crossover 

Resistor 

Motor Current 
Sense 

Ohmite 

Variable Resistor, 
.0-.5 ohms. 
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E.3 Torque Motor/Servo-Amp 


The digital to analog converter generated voltage 
signals proportional to the desired motor torque. The 
commutated DC torque motor was driven by a servo-amp 
configured in current mode. This meant that a current was 
driven through the amplifier load (DC torque motor) 
proportional to the input voltage. This implementation uses 
a sense resistor on the output terminal of the motor to 
monitor current. The connections and configuration for this 
mode of operation are shown in figure E-6. 



Figure E-6. Motor/ Amplifier Current Mode Configuration. 




APPENDIX F 


CONTROLLER IMPLEMENTATION 

This section covers the issues and structures of the 
real time controller implementation, key hardware elements, 
and software. Specific features, and methods written into 
the software which were particularly useful for this system 
are discussed. Additionally, the actual code utilized on the 
IBM Series/ 1 to implement the LQR controller designs of 
chapter 7 in real time is presented here as an example for 
future programmers. The basic system structure and utilities 
are described as well as the process required to compile, 
link, and execute the software. 

F.l Software Development 

The software evolved over the course of the controller 
implementations into a compromise between initial 
objectives, and performance requirements. Throughout the 
software design the following goals were used as guides: 

• Intelligibility - easily understandable source 

code, high level of documentation. High level 

language . 

• Modularity - separation of code into distinct 

functional modules, subroutines etc., common 
to multiple controllers. 

• Operator Input/Output - Terminal supported 

parameter adjustment. 
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• Speed - Fast execution times, resulting in short 
cycle times which reduce the impact of 
discretization 

Applying these goals to a real-time implementation can 
not be accomplished in an abstract form. Specific features 
of the hardware, and available software compatible with the 
hardware become extremely important in making software 
generation. 

Initially the controllers were implemented in Fortran, 
this is a high level language familiar to most engineering 
students. Difficulties were encountered by the vary system 
transparency afforded by a high level language. System 
overhead primarily due to provisions for multi-level, multi- 
tasking options resulted in execution speeds far short of 
processor capabilities. Additionally special features of the 
analog input and output, and terminal devices were not 
easily accessed. The combination of these factors led to 
abandonment of Fortran as the language for implementation. 

The second generation of control software was written 
in IBM's system language. Event Driven Language (EDL). EDL 
is a moderately high level language providing IF-THEN, DO- 
WHILE structures, as well as subroutines support, yet 
closely resembles assembly language statements. EDL is also 
tailored to the specific hardware elements providing full 
use the device features. The difficulty encountered in this 
iteration of the controllers was primarily speed of 


execution. Although EDL provided better access to the 
hardware devices, system overhead was still extensive and 
execution times were still much slower than indicated by 
summing processor instruction times. 

The third generation of software was a hybrid of EDL 
and Assembly languages. EDL was utilized for controlling the 
system interfaces, and the computations were executed in 
assembly language routines. The execution times for this 
software hybrid came close to realizing the capabilities of 
the processor. This compromise leaned more to the 
performance requirements and modularity goals fob the 
software, and less toward the intelligibility goal. However, 
with alot of annotation, the time required for 
familiarization is minimized. 

F.2 Hardware Features 

Special hardware functions were, found to be extremely 
helpful in implementation of the control laws. Hardware 
floating point computations were used throughout the 
controller implementations. Depending upon the control law 
design procedure and the input quantities being investigated 
at any one time, the state gains varied a full order of 
magnitude. Use of floating point instructions avoided 
extensive rescaling required with integer computations to 
retain high accuracy and resolution. 

Another feature found to be extremely useful in 



investigating control strategies, was static screen support 
for menu driven parameter adjustment using the terminal. 
Many parameters needed to be input to the system and updated 
for the various controllers. Static screens supported menu 
type data entry providing easy access, and verification of 
the memory locations holding key parameters. 

F . 3 IBM Series/1 

The IBM Series/1 utilized for the experiments is a 
micro-processor based computing system supporting several 
processors, and a wide variety of peripheral devices. For a 
complete list of processors, cards, and subsystems supported 
refer to "Series/1 Digest"[Fl] . Only the specific 
configuration used in the experiment will be discussed here. 

There are several unique features to the Series/ 1 in 
addition to the floating point hardware and terminal support 
mentioned above. These additional features were not required 
for the sequential implementation of the controllers, but 
will be mentioned to provide reference to the available 
resources. The processor has separate I/O, and memory 
channels, 213 programable interrupts from the I/O channel, 
and four hardware levels for rapid execution interrupt 
processes . 

The capabilities, and hardware of the Series/1 
utilized for the experiments are summarized in table F-l. 


Table F-l . Series/1 Configuration 


Micro-processor 

Cycle Time 
Instruction set 
Storage 

Floating Multiply 
I/O Channel 


Mass Storage 

Hard Disk 
Floppy 

Input and Output 

Analog Input 


Analog Output 


Digital I/O 


Communications 

Serial Async. 


Model 4955-F, 16 bit 
220 nano-sec 
160 basic set 
512 kilo-byte 
19 micro-sec 

256 addressable, 1.65 mega- 
byte/sec throughput. 

Model 4964 
64 mega-byte 
256 kilo-byte 

Sensor I/O no. 4982 

Solid State 

16 differential points, 9600 

samples/sec 

Reed Relay 

8 differential points ,200 
samples /sec 


Solid State 
2 differential points, 

20 micro-sec settling time. 


Isolated, 16 points 
Non-Isolated, 16 points 


8 ports, ASCII, EBCDC 


F3.1 Utilities and Program Preparation 

The Series/1 supports an IBM programming 
language/operating system called the Event Driven Executive, 
which refers to its ability to identify and service 
interrupt requests. This comes with a lot of useful utility 
programs for system generation, diagnostics, and data 
management. Additionally, editing, compiling, linking, and 


interactive debugging is well supported. 


All the utility programs can be executed individually 
using the relocating loader routine $L, or a "mothering" 
program, $SMMAIN, which provides menu driven interfaces 
with which the utility routines can be invoked. 
Documentation on the utility programs and there use is 
contained in "Operator commands and Utilities 
Reference" [F2 ] . An example of the procedure for compiling 
will be given with each program, and a linking procedure 
will follow the source codes for each controller. 


F . 4 Deterministic Regulator 

The source code for the regulator is broken into 
several assembly language subroutines joined together. by a 
driver/initialization routine written in IBM's Event Driven 
Language EDL. The routines, their names, and functions are 
summarized below: 


LUENEDX ( EDL ) 

CONV(ASM) 

UPDATE (ASM) 
CNTRL ( ASM ) 

EST(ASM) 


Drive parameter initialization menus, 
call the assembly level subroutines, 
provide interrupt capability 
onvert input values, reconstruct 
modal variables. 

Update state estimate. 

Execute control law and convert 
output 

Estimate state variables. 



F.5 Program Generation 


This section discusses the process required to 
generated executable code for the Series/1. These steps are 
straightforward, typical of all processors, but may be less 
transparent on the Series/ 1 than some programmers are 
familiar with. The steps consist of source code generation 
using a line or screen editor. The Series /I provides a 
reasonable screen editor $FSE which can be loaded directly, 
or via the system manager. Fortran, EDL, and Assembly code 
source code can all be generated with this editor. The 
source modules must be compiled individually into object 
code. The system is provisioned with compilers for all three 
languages, $FORT for Fortran, $EDXASM for EDL, and $S1ASM 
for the Assembly source code which can again be invoked 
directly at the system prompt, or with the session manager. 

The compiled objects must be linked with each other, 
and system library files into a relocatable module for 
execution. The modules are linked with the utility program, 
$EDXLINK, which as with the other system utilities can be 
loaded directly or with the session manager. 

The program generation sequence is summarized in table 
F-2, for the case of the deterministic regulator. The source 
files, temporary holding files set aside for the object 
modules, and linking instructions are included. 
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Table F-2. Program Generation Sequence 

Step 1. Generate Source Code Modules (Editor - $FSE,EDX003) 
Event Driven Language Driver - LUENEDX , GORDON 

• Assembly Code Modules - CONV1, GORDON 

- UPDATE, GORDON 

- CNTRL, GORDON 

- EST, GORDON 

Step 2. Assemble Object Modules 

• Event Driven Language Modules (Compiler - $EDXAS 

M, EDX003 ) 

. Compile LUENEDX , GORDON to ASMWKl , GORDON 
Assembly Language Modules (Compiler - $S1ASM, EDX003 ) 
compile CONV1, GORDON to ASMWK2, GORDON 
UPDATE , GORDON ASMWK3 , GORDON 

• CNTRL, GORDON ASMWK4 , GORDON 

• EST , GORDON ASMWK5 , GORDON 

Step 3. Link Executable Module (Linker - $EDXLINK,EDX003 ) 
Assembled Object Modules 

Include ASMWKl , GORDON 

• Include ASMWK2 , GORDON 

Include ASMWK 3 , GORDON 
Include ASMWK 4 , GORDON 
Include ASMWK 5 , GORDON 

Access System Library 

• Autocall $ AUTO , ASMLIB 

Perform Linkage 

Link EXEC, GORDON Replace End 

Step 4 . Execute Program 

• At System Prompt Utilize Relocating Loader on 

Executable Module 

$L EXEC, GORDON 

Then Provide Input, and Output Data Files at Prompts 
INI 0, GORDON 

INI 0, GORDON (Write Modifications to Same 

• File) 

F.5.1 Routine LUENEDX 

This file can be compiled using the utility routine 
$EDXASM. This routine is most easily accessed through the 
program preparation facility of $SMMAIN. One word of 

• caution, destination files for the compiled objects must be 

allocated prior to compilation, or the object will not be 
stored. To avoid generation of excess object files on the 
hard disk, ASMWKl on volume CONTROL was allocated as the 


typical destination for the compilation of LUENEDX . 

(START OF FILE LUENEDX) 

************************************************************ 
***** *** 

* EDX DRIVER PROGRAM WITH INITIALIZATION ROUTINE, AND I/O *** 

***** *** 

************************************************************ 

* 

REDOO PROGRAM START , DS= ( ( INPUT ,??),( OUTPUT ,??)), FLOAT=YES 
* 

* KEYBOARD INTERUPT ROUTINE DEFINITION TO STOP CONTROLLER 


* 

ATTNLIST 

(X,QUIT) 

QUIT 

MOVE 

ENDATTN 

FLAG , 1 


EXTRN 

EST , CONV , UPDATE , CNTRL 


EXTRN 

$ IMOPEN , $ IMDEFN , $ IMPROT 

TERM 

* 

IOCB 

SCREEN=STATIC 


************************************************************ 

* DEFINE THE ANALOG INPUT AND OUTPUT PORTS 

* THETA, STRAIN1,STRAIN2, VELOCITY 
************************************************************ 

* 

* ANGLE SENSOR 

* 

IODEF All, ADDRESS=63 ,POINT=0 ,RANGE=5V 


* STRAIN GAGES 

* 


* 

IODEF 

IODEF 

AI 2 , ADDRESS=6 3 , POINT=l , RANGE=500MV 
AI 3 , ADDRESS=6 3 , POINT=2 , RANGE=5 0 OMV 

* 

* 

VELOCITY SENSOR 

* 

IODEF 

AI 4 , ADDRESS=6 3 , POINT=3 , RANGE= 5 V 

* 

* 

THE DESIRED 

END POINT INPUT 

it 

IODEF 

AI 5 , ADDRESS=6 3 , POINT=4 , RANGE=5V 

it 

it 

DITHER SIGNAL 


it 

IODEF 

AI 6 , ADDRESS=6 3 , POINT=5 , RANGE=5V 

* 

it 

THE TORQUE 

OUTPUT A/D DEVICE 


IODEF 

AOl , ADDRESS=6 4 , POINT=0 


* 

* 


SIGNAL OUTPUT PORT 




IODEF 


AO 2 , ADDRESS=6 4 , POINT=l 


* 

************************************************************ 


* MAIN MENU DRIVER 

* 

************************************************************ 

* 


CALL THE PREPARED SCREEN IMAGE 


* 

START EQU 

* 

LMENU CALL 

$IMOPEN, ( DSNAME1 ) , (DISKBUF) , ( TERMTYPE ) 


ENQT 

TERM 


CALL 

$IMPROT, (DISKBUF) , ( FTABLE ) 

* 

TERMCTRL 

DISPLAY 

* 

* 

READ THE SELECTION INTEGER CHOICE 


PRINTNUM 

ZERO , FORMAT= (1,0,1), LINE=1 6 , SPACES=2 6 


PRINTEXT 

LINE=16 , SPACES=26 


TERMCTRL 

DISPLAY 


WAIT 

KEY 


GETVALUE 

CHOICE , FORMAT^ (1,0,1), LINE=1 6 , SPACES=2 6 


IF 

( CHOICE , EQ , 1 ) , GOTO , LINPUT 


IF 

( CHOICE , EQ , 2 ) , GOTO , LGAIN 


IF 

( CHOICE , EQ , 3 ) , GOTO , LOBSVR 


IF 

( CHOI CE , EQ , 4 ) , GOTO , LINCOEF 


IF 

( CHOI CE , EQ , 5 ) , GOTO , LUPDATE 


IF 

( CHOICE , EQ , 6 ) , GOTO , LZERO 


IF 

( CHOICE , EQ , 7 ) , GOTO , LOUTPT 


IF 

( CHOICE , EQ , 8 ) , GOTO , LXQT 


IF 

( CHOICE , EQ , 9 ) , GOTO , LEND 

* 

GOTO LMENU 


* 

ROUTINE TO INPUT DATA SET OF PARAMETERS 

* 

* 

CALL 

THE PREPARED SCREEN 

LINPUT CALL 

$IM0PEN, ( DSNAME3 ) , (DISKBUF) , (TERMTYPE) 


ENQT 

TERM 


CALL 

IIMPROT, (DISKBUF) , (FTABLE) 

* 

TERMCTRL 

DISPLAY 

if 

if 

READ THE INPUT 

DATA SET NAME AND VOLUME 


READ 

DS1 ,DATBUF , 2 


MOVE 

FN , DATBUF , ( 2 5 6 , BYTES ) 

if 

GOTO 

LMENU 


ROUTINE VIEW/ ALTER GAIN VECTOR 

ic 

CALL 

THE PREPARED SCREEN 



204 


LGAIN 

CALL 

$IMOPEN, ( DSNAME2 ) , (DISKBUF) , (TERMTYPE) 


ENQT 

TERM 


CALL 

$ IMPROT , ( DI SKBUF ) , ( FTABLE ) 

* 

TERMCTRL 

DISPLAY 


• * FILL IN THE DEFAULT VALUES 

* 


PRINTNUM FN+56 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=3 , SPACES=4 0 
PRINTNUM FN+60 ,FORMAT=( 12 , 6 ,E) ,TYPE=F,LINE=5 ,SPACES=40 
PRINTNUM FN+6 4 , FORMAT= ( 1 2 , 6 , E ) ,TYPE=F,LINE=7 ,SPACES=40 
PRINTNUM FN+68 , FORMAT= ( 12 , 6 , E) , TYPE=F , LINE=9 , SPACES=4 0 
• PRINTNUM FN+72 , FORMAT= ( 12 , 6 , E) ,TYPE=F, LINE=11 , SPACES=40 

PRINTNUM FN+7 6 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=4 0 


* 

* 

* 


* 


POSITION THE CURSOR, WAIT FOR THE ENTER KEY, GET THE 
VALUE 


PRINTEXT 

TERMCTRL 

WAIT 

GETVALUE 

GETVALUE 

GETVALUE 

GETVALUE 

GETVALUE 

GETVALUE 

GOTO 


LINE=14,SPACES=33 

DISPLAY 

KEY 

FN+56 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=3 , SPACES=4 0 
FN+6 0 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=5 , SP ACES=4 0 
FN+6 4 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=7 , SPACES=4 0 
FN+6 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=9 , SPACES=4 0 
FN+7 2 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE= 1 1 , SP ACES= 4 0 
FN+7 6 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=4 0 
LMENU 


ROUTINE TO VIEW/ALTER OBSERVOR PARAMETERS 


LOBSVR 


ENQT 

TERMCTRL 

DEQT 

CALL 

ENQT 

CALL 

TERMCTRL 


TERM 

BLANK 

$IMOPEN, (DSNAME4) , (DISKBUF) , (TERMTYPE) 
TERM 

$IMPROT, (DISKBUF) , ( FTABLE ) 

DISPLAY 


PRINTNUM FN , FORMAT= ( 12 , 6 , E) , TYPE=F , LINE=7 , SPACES=4 
PRINTNUM FN+4 ,FORMAT=( 12 , 6 ,E) ,TYPE=F,LINE=7,SPACES=21 
PRINTNUM FN+2 4 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=7 , SPACES=4 9 
PRINTNUM FN+2 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=9 , SPACES=4 
PRINTNUM FN+3 2 , FORMAT= ( 1 2 , 6 , E ) ,TYPE=F,LINE=9,SPACES=21 
PRINTNUM FN+5 2 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=9 , SPACES=4 9 
PRINTNUM FN+8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=8 
PRINTNUM FN+1 2 , FORMAT= ( 1 2 , 6 , E ) ,TYPE=F,LINE=13 ,SPACES=25 
PRINTNUM FN+1 6 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=4 2 
PRINTNUM FN+2 0 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=5 9 
PRINTNUM FN+3 6 ,FORMAT=( 12 , 6 ,E) ,TYPE=F,LINE=15 ,SPACES=8 

PRINTNUM FN+4 0 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=2 5 
PRINTNUM FN+4 4 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=4 2 
PRINTNUM FN+ 4 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE= 1 5 , SP ACES= 5 9 
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PRINTEXT LINE=19,SPACES=30 
TERMCTRL DISPLAY 
WAIT KEY 

GETVALUE FN , FORMAT= ( 12,6,E) ,TYPE=F,LINE=7 ,SPACES=4 

GETVALUE FN+4 , FORMAT= ( 12,6, E) , TYPE=F , LINE=7 , SPACES=2 1 
GETVALUE FN+24 , FORMAT=( 12 , 6 ,E) ,TYPE=F,LINE=7 ,SPACES=49 
GETVALUE FN+ 2 8 , FORMAT= ( 12,6,E) ,TYPE=F,LINE=9 ,SPACES=4 

GETVALUE FN+32 , FORMAT=( 12 , 6 ,E) , TYPE=F , LINE=9 , SPACES=21 
GETVALUE FN+5 2 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=9 , SPACES=4 9 

GETVALUE FN+8 , FORMAT= ( 12 , 6 , E ) , TYPE=F , LINE=13 , SPACES=8 

GETVALUE FN+1 2 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=1 3 , SPACES=2 5 
GETVALUE FN+1 6 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=4 2 
GETVALUE FN+20 , FORMAT= ( 12 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=5 9 
GETVALUE FN+3 6 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=15 , SPACES=8 
GETVALUE FN+4 0 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=2 5 
GETVALUE FN+4 4 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=1 5 , SPACES=4 2 
GETVALUE FN+4 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=5 9 
DEQT 

GOTO LMENU 
LUPDATE ENQT TERM 

TERMCTRL BLANK 

DEQT 

CALL $ IMOPEN , ( DSNAME5 ) , ( DI SKBUF ) , ( TERMTYPE ) 

ENQT TERM 

CALL $ IMPROT , ( DI SKBUF ) , ( FT ABLE ) 

TERMCTRL DISPLAY 

PRINTNUM FN+1 4 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=8 
PRINTNUM FN+1 5 2 , FORMAT= ( 1 2 , 6 , E ) ,TYPE=F,LINE=15 ,SPACES=8 
PRINTNUM FN+1 5 6 , FORMAT= ( 1 2 , 6 , E ) ,TYPE=F,LINE=13 ,SPACES=25 
PRINTNUM FN+1 6 0 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=1 5 , SPACES=2 5 
PRINTNUM FN+1 6 4 , FORMAT= (12 , 6,E) , TYPE=F , LINE=1 3 , SPACES=4 2 
PRINTNUM FN+1 6 8 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=1 5 , SPACES=4 2 
PRINTNUM FN+1 7 2 , FORMAT= ( 12 , 6 ,E) , TYPE=F , LINE=1 3 , SP ACES= 5 9 
PRINTNUM FN+1 7 6 , FORMAT=( 12 , 6 , E ) ,TYPE=F, LINE=15 , SPACES=59 
PRINTEXT LINE=19 , SPACES=30 
TERMCTRL DISPLAY 
WAIT KEY 

GETVALUE FN+1 4 8 , FORMAT= ( 12, 6 ., E) ,TYPE=F,LINE=13 ,SPACES=8 
GETVALUE FN+ 152, F0RMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE= 1 5 , SPACES=8 
GETVALUE FN+1 5 6 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 3 , SPACES=2 5 
GETVALUE FN+1 6 0 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=2 5 
GETVALUE FN+1 6 4 , FORMAT= ( 12, 6, E) , TYPE=F , LINE=1 3 , SPACES=4 2 
GETVALUE FN+1 6 8 , FORMAT= ( 1 2 , 6 , E ) , TYPE=F , LINE=1 5 , SPACES=4 2 
GETVALUE FN+172 ,FORMAT=( 12, 6 , E) ,TYPE=F,LINE=13 ,SPACES=59 
GETVALUE FN+1 7 6 , FORMAT= ( 12 , 6 , E ) ,TYPE=F, LINE=15 , SPACES=59 

DEQT 

GOTO LMENU 

* 

* ROUTINE TO VIEW/ALTER INPUT COEFFICIENTS 

* 

LINCOEF ENQT 


TERM 
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* 

* 

* 


* 

* 

* 


TERMCTRL 

DEQT 

CALL 

ENQT 

CALL 

TERMCTRL 


BLANK 

$IMOPEN, (DSNAME6) , ( DISKBUF ) , (TERMTYPE) 
TERM 

$IMPROT, (DISKBUF) , ( FTABLE) 

DISPLAY 


FILL IN THE DEFAULT VALUES 

PRINTNUM FN+80,FORMAT=(12,6,E 
PRINTNUM FN+8 4 , FORMAT= ( 1 2 , 6 , E 

PRINTNUM FN+8 8 , FORMAT=( 12 , 6 , E 

PRINTNUM FN+92 ,FORMAT=( 12 , 6 , E) 
PRINTNUM FN+96 ,FORMAT=( 12 , 6 ,E) 
PRINTNUM FN+100 ,FORMAT=( 12 , 6 ,E) 
PRINTNUM FN+104 ,FORMAT=( 12 , 6 ,E) 
PRINTNUM FN+108 , FORMAT=( 12 , 6 ,E) 
PRINTEXT LINE=21 , SPACES=3 4 
TERMCTRL DISPLAY 

WAIT KEY 

GETVALUE FN+ 8 0 , FORMAT= (12,6 
GETVALUE FN+8 4 , FORMAT= (12,6, 
GETVALUE FN+8 8 , FORMAT= (12,6 
GETVALUE FN+92 , FORMAT= (12,6, 
GETVALUE FN+96 , FORMAT= (12,6, 
GETVALUE FN+1 0 0 , FORMAT= (12,6, 
GETVALUE FN+1 0 4 , FORMAT= (12,6, 
GETVALUE FN+1 0 8 , FORMAT= (12,6, 
TERMCTRL BLANK 
DEQT 

GOTO LMENU 


) , TYPE=F 
) ,TYPE=F 
) , TYPE=F 
, TYPE=F, 
, TYPE=F , 
, TYPE=F , 
, TYPE=F , 
,TYPE=F, 


,LINE=5 
,LINE=7 
, LINE=9 
LINE=11 
LINE=13 
LINE=15 
LINE=17 
LINE=19 


, E ) ,TYPE=F 
E) , TYPE=F , 
, E ) , TYPE=F 
E) ,TYPE=F, 
E) , TYPE=F , 
E) , TYPE=F , 
E ) , TYPE=F , 
E) ,TYPE=F, 


,LINE=5 
LINE=7 , 
,LINE=9 
LINE=11 
LINE=13 
LINE=15 
LINE=17 
LINE=19 


,SPACES=25 
,SPACES=25 
,SPACES=25 
, SPACES=25 
,SPACES=25 
,SPACES=25 
,SPACES=25 
,SPACES=25 


,SPACES=25 

SPACES=25 

,SPACES=25 

,SPACES=25 

,SPACES=25 

,SPACES=25 

,SPACES=25 

,SPACES=25 


ROUTINE TO OUTPUT SELECTED PARAMETERS 


LOUTPT 


ENQT 

TERM 

TERMCTRL 

BLANK 

DEQT 

CALL 

$ IMOPEN, (DSNAME8) , (DISKBUF) 

ENQT 

TERM 

CALL 

$ IMPROT , ( DI SKBUF ) , ( FTABLE ) 

TERMCTRL 

DISPLAY 

MOVE 

DATBUF , FN , ( 2 5 6 , BYTES ) 

WRITE 

DS2,DATBUF, 2 

DEQT 

GOTO 

LMENU 


*********************************************************** 

****** 

****** SYSTEM INITIAL ZEROING ROUTINE 

****** 

*********************************************************** 
LZERO CALL $ IMOPEN , ( DSNAME7 ) , ( DI SKBUF ) , ( TERMTYPE ) 
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ZZZZ 


* 


ENQT 

CALL 

TERMCTRL 

SBIO 

SBIO 

SBIO 

SBIO 


TERM 

$IMPROT, ( DISKBUF ) , ( FT ABLE ) 

DISPLAY 

All, AN 

AI2, AN+2 

AI3 , AN+4 

AI4 , AN+6 


* CALL THE CONVERSION ROUTINE TO TRANSFORM THE INPUTS 

* 


USER CONV , PARM= ( AN , FN ) 

PRINTNUM FN+112 , FORMAT= ( 12 , 6 , E ) , TYPE=F , LINE=4 , SPACES=3 0 
PRINTNUM FN+124 , FORMAT=( 12 , 6 ,E) ,TYPE=F,LINE=6 , SPACES=30 
PRINTNUM AN+2 , FORMAT=( 6,0,1) ,LINE=8 , SPACES=30 
PRINTNUM AN+4 , FORMAT 3 ( 6,0,1), LINE=10 , SPACES=30 

PRINTNUM ZERO , FORMAT= (1,0,1) , LINE=12 , SPACES=31 

LINE=12,SPACES=31 
DISPLAY 
KEY 

ZM1 , FORMAT= (1,0,1), LINE=1 2 , SPACES=3 1 
( ZM1 , EQ , 1 ) , GOTO , LMENU 
ZZZZ 


LXQT 

STRCN 


PRINTEXT 

TERMCTRL 

WAIT 

GETVALUE 

IF 

GOTO 
DEQT 
CALL 
ENQT 
CALL 
DEQT 


$IMOPEN, ( DSNAME9 ) , (DISKBUF) , ( TERMTYPE ) 
TERM 

$ IMPROT , ( DI SKBUF ) , ( FTABLE ) 


a********************************************************** 


* * * * * 
***** 
***** 


REDUCED DYNAMIC ORDER LUENBERGER OBSERVER/ CONTROLLER 


*********************************************************** 

* 

* THE SECTION WHICH READS THE ANALOG INPUT VALUES 

SBIO All, AN 

SBIO AI2, AN+2 

SBIO AI3 , AN+4 

SBIO AI4 , AN+6 

SBIO AI5, AN+10 

SBIO AI6 , AN+12 

CALL THE ASEMBLER VERSION OF THE ESTIMATOR AND THE 
CONTROLLER 


LOOP1 


USER 

CONV , PARM= ( AN , FN ) 

USER 

UPDATE , PARM= ( AN , FN ) 

USER 

CNTRL , PARM= ( AN , FN ) 

SBIO 

AOl , AN+8 

USER 

EST , PARM= ( AN , FN ) 

IF 

( FLAG , NE , 0 ) , GOTO , RTN 
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GOTO LOOP1 

RTN SBIO AOl , ZERO 

MOVE FLAG , 0 

ENQT TERM 

TERMCTRL BLANK 
• GOTO LMENU 


LEND PROGSTOP 

* 

********************************************************** 


FLAG DATA F ' 0 ' 

ZERO DATA F ' 0 ' 

CHOICE DATA F'O’ 

ZM1 DATA F'O' 

INL DATA F'O' 

AN DATA 12F ' 0 ' 

INSC DATA F ' 125 ' 

*JOINT EQU AN 

♦STRAIN1 EQU AN+2 

♦STRAIN2 EQU AN+4 

*VEL EQU AN+6 

*TORQ EQU AN+8 

*DESANGLE AN+10 

♦DITHER SIGNAL AN+12 
* AM+14 

FN DATA 64E '0.0' 


*F11 

EQU 

FN 

OBSERVER VELMOD1 

DEPENDENCE 

LAST 

VELMODl 

*F12 

EQU 

FN+4 

M 

ft 

ii 

ii 

VELMOD2 

*G11 

EQU 

FN+8 

ft 

ft 

ii 

ii 

ANGLE 

*G12 

EQU 

FN+12 

tf 

II 

ii 

ii 

MODI 

*G13 

EQU 

FN+16 

M 

»» 

ii 

ii 

MOD 2 

*G14 

EQU 

FN+20 

»t 

»» 

ii 

ii 

JOINTVEL 

*B1 

EQU 

FN+24 

ft 

If 

ii 

ii 

TORQUE 

*F21 

EQU 

FN+28 

OBSERVOR VELMOD2 

DEPENDENCE 

LAST 

VELMODl 

*F22 

EQU 

FN+32 

It 

If 

ii 

ii 

VELMOD2 

*G21 

EQU 

FN+36 

ft 

If 

ii 

ii 

ANGLE 

*G22 

EQU 

FN+40 

It 

II 

ii 

ii 

MODI 

*G23 

EQU 

FN+4 4 

ft 

If 

it 

ii 

MOD 2 

*G24 

EQU 

FN+4 8 

It 

II 

ii 

ii 

JOINTVEL 

*B2 

EQU 

FN+52 

It 

II 

ii 

ti 

TORQUE 

*K1 

EQU 

FN+56 

JOINT 

ANGLE GAIN 



*K2 

EQU 

FN+60 

MOD 1 

AMPLITUDE GAIN 



*K3 

EQU 

FN+64 

MOD 2 

AMPLITUDE GAIN 



*K4 

EQU 

FN+68 

JOINT 

VELOCITY 

GAIN 



*K5 

EQU 

FN+72 

MOD 1 

VELOCITY 

GAIN 



*K6 

EQU 

FN+76 

MOD 2 

VELOCITY 

GAIN 




♦CONV1 EQU FN+80 JOINT ANGLE INPUT CONVERSION 
♦CONV2 EQU FN+84 STRAIN 1 MODI INPUT CONVERSION 

*CONV3 EQU FN+88 STRAIN 2 MODI INPUT CONVERSION 

♦CONV4 EQU FN+92 STRAIN 1 MOD2 INPUT CONVERSION 

♦CONV5 EQU FN+96 STRAIN 2 MOD2 INPUT CONVERSION 
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*CONV6 

EQU 

FN+100 

TACHOMETER INPUT CONVERSION 

*CONV7 

EQU 

FN+104 

TORQUE 

OUTPUT CONVERSION 

*CONV8 

EQU 

FN+108 

INPUT 

SIGNAL CONVERSION 

*FTHET 

EQU 

FN+112 

JOINT 

ANGLE FLOATING POINT 

*FMODl 

EQU 

FN+116 

AMPLITUDE 

MODE1 FLOATING POINT 

*FMOD2 

EQU 

FN+120 

AMPLITUDE 

MODE 2 FLOATING POINT 

*FOMEG 

EQU 

FN+124 

JOINT 

VELOCITY FLOATING POINT 

*ESTV1 

EQU 

FN+128 

ESTIMATED 

MODI VELOCITY FLOATING 

*ESTV2 

EQU 

FN+132 

ESTIMATED 

MOD 2 VELOCITY FLOATING 

*TORQ 

EQU 

FN+136 

TORQUE 

FLOATING POINT 

*OUTPT 

EQU 

FN+140 




*ZERO 

EQU 

EN+144 




* 

EQU 

FN+148 

UPDATE 

GAIN LI 

* 

EQU 

FN+152 

II 

II 

L2 

* 

EQU 

FN+156 

II 

If 

L3 

* 

EQU 

FN+160 

II 

If 

L4 

* 

EQU 

FN+164 

If 

If 

L5 

* 

EQU 

FN+168 

If 

ff 

L6 

* 

EQU 

FN+172 

If 

ft 

L7 

* 

EQU 

FN+176 

If 

If 

L8 

* 

EQU 

FN+180 



- 

* 

EQU 

FN+184 




DATBUF 

BUFFER 

256, BYTES 



BDS 

BUFFER 

25 8, BYTES 



DISKBU 

BUFFER 

1024, BYTES 



FT ABLE 

BUFFER 

15, WORDS 



RDATl 

BUFFER 

512, BYTES , INDEX= 

INDX1 

INSET 

TEXT 

LENGTH 1 

=20 



OUTSET 

TEXT 

LENGTH 1 

=20 




DSNAME1 TEXT 
DSNAME2 TEXT 
DSNAME3 TEXT 
DSNAME4 TEXT 
DSNAME5 TEXT 
DSNAME6 TEXT 
DSNAME7 TEXT 
DSNAME8 TEXT 
DSNAME9 TEXT 
TERMTYPE DATA 
MEAS DATA 

ENDPROG 

END 

(END OF FILE LUENEDX ) 


'MENUSCR, GORDON' 
'GAINSCR, GORDON' 

' INPUTSCR , GORDON ' 
' OBSVRSCR , GORDON ' 
' UPD ATS CR, GORDON' 
'COEFSCR, GORDON' 
'ZEROSCR, GORDON’ 
'OUTPTSCR, GORDON' 
'LUENSCR, GORDON' 
C ' 4978 ' 

4F ' 0 ' 


F . 5 . 2 Routine CONV 


This routine is compiled using the utility routine $S1ASM. 
The program preparation facility accessed via $SMMAIN is the 
easiest method for its execution. The file ASMWK2 on volume 


CONTROL of the harddisk was reserved as the destination file 
for the compiled object. 

(START OF FILE CONV) 

*********************************************************** 

***** 

***** INPUT CONVERSION/RECONSTRUCTION ROUTINE 

***** 

*********************************************************** 



START 

EXTRN 

ENTRY 

RETURN 

CONV 


CONV 

EQU 

* 



FMVC 

2 ( R1 , 0 ) * , FR3 

STRAIN1 


FM 

84 (R1 , 2 ) * , FR3 

CONV2 * STRAIN1 


FMVC 

4 ( R1 , 0 ) * , FR2 

STRAIN 2 


FM 

88 (Rl, 2 ) * , FR2 

CONV3*STRAIN2 


FA 

FR2 , FR3 

MODE1 


FMV 

FR3, 116 (Rl , 2 ) * 

SAVE MODEl 


FMVC 

2(R1,0)*,FR2 

STRAIN1 


FM 

92 (Rl , 2 ) * , FR2 

CONV4*STRAINl 


FMVC 

4 ( Rl , 0 ) * , FR1 

STRAIN 2 


FM 

96(R1,2)*,FR1 

CONV5*STRAIN2 


FA 

FR1 , FR2 

MODE 2 


FMV 

FR2, 120 ( Rl , 2 ) * 

SAVE MODE 2 


FMVC 

( Rl , 0 ) * , FR1 

THETA 


FM 

80 (Rl , 2 ) * , FR1 

CONV 1* THETA 


FMV 

FR1, 112 ( Rl , 2 ) * 

SAVE FTHETA 


FMVC 

6 ( Rl , 0 ) * , FR1 

OMEGA 


FM 

100 ( Rl , 2 ) * , FR1 

CONV 6 * OMEGA 

(END 

FMV 
ABI 
’ B 
END 

OF FILE CONV) 

FR1 , 124 ( Rl , 2 ) * 

4 , Rl 

RETURN 

SAVE FOMEGA 


F.5.3 Routine UPDATE 


This routine is compiled using the utility routine $S1ASM. 
The program preparation facility accessed via SSMMAIN is the 
easiest method for its execution. The file ASMWK3 on volume 
CONTROL of the harddisk was reserved as the destination file 
for the compiled object. 
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4 


(START OF FILE UPDATE) 

*★★*★****★★★★★★★★★★★*****************★*★****★****★****★★*** P 
★ * * * * 

***** UPDATE FOR LUENBERGER OBSERVER 

***** 

*********************************************************** 

START 

EXTRN RETURN -.DISPLACEMENT LISTING IN M2 SO 

ENTRY UPDATE 

UPDATE EQU * 

* 

** UPDATE ESTIMATE OF MODE ONE VELOCITY 


FMV 

112 (R1 , 2 ) * ,FR3 

: FTHETA 

FM 

148 (R1 , 2 ) * , FR3 

: UPDATE LI* FTHETA 

FMV 

116 (R1 , 2 ) * , FRl 

: FMODE1 

FM 

156 (R1 , 2 ) * , FR1 

: UPDATE L3*FMODl 

FA 

FR1 , FR3 

: ADD TO UPDATE Z1 

FMV 

120 (R1 , 2 ) * , FRl 

: FMODE2 

FM 

164( R1 , 2 ) * , FR1 

: UPDATE L5*FMODE2 

FA 

FRl , FR3 

: ADD TO UPDATE Z1 

FMV 

124(R1 , 2 ) * ,FR1 

: FOMEGA 

FM 

172 (R1 , 2 ) * ,FR1 

: UPDATE L7* FOMEGA 

FA 

FRl , FR3 

: ADD TO UPDATE Z1 

FA 

1 2 8 ( R1 , 2 ) * , FR3 

-.ADD EST VEL MODE1 

FMV 

FR3 , 128 ( R1 , 2 ) * 

: SAVE UPDATE Zl 


** BEGIN UPDATE ESTIMATE OF MODE TWO VELOCITY 


FMV 

112 (R1 , 2 ) * ,FR3 

FTHETA 


FM 

152(R1 , 2 ) * ,FR3 

UPDATE 

L2* FTHETA 

FMV 

1 1 6 ( R1 , 2 ) * , FRl 

FMODE1 


FM 

1 6 0 ( R1 , 2 ) * , FRl 

UPDATE 

L4*FMODl 

FA 

FRl , FR3 

ADD TO 

UPDATE Z2 

FMV 

120 (R1 , 2 ) * , FRl 

FMODE2 


FM 

168 (R1 , 2 ) * , FRl 

UPDATE 

L6*FMODE2 

FA 

FRl , FR3 

ADD TO 

UPDATE Z2 

FMV 

1 2 4 ( R1 , 2 ) * , FRl 

FOMEGA 


FM 

176 (R1 , 2 ) * ,FR1 

UPDATE 

L8*FOMEGA 

FA 

FR1,FR3 

ADD TO 

UPDATE Z2 

FA 

1 3 2 ( R1 , 2 ) * , FR3 : ADD EST 

VEL MODE1 

FMV 

FR3 , 132 ( R1 , 2 ) * 

:SAVE UPDATE Z2 

ABI 

4 ,R1 



B 

RETURN 



END 





F . 5 . 4 Routine CNTRL 

This routine is compiled using the utility routine $S1ASM. 
The program preparation facility accessed via $SMMAIN is the 
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easiest method for its execution. The file ASMWK3 on volume 
CONTROL of the harddisk was reserved as the destination file 


for the compiled object. 


(START OF FILE CNTRL ) 

*********************************************************** 

***** 

***** CONTROL LAW FOR LUENBERGER/ KALMAN FILTER 

***** 

*********************************************************** 


START 

EXTRN 

ENTRY 

CNTRL EQU 
FMV 
FM 
FMV 
FM 
FA 
FMV 
FM 
FA 
FMV 
FM 
FA 
FMV 
FM 
FA 
FMV 
FM 
FA 
FMVC 
FM 
FA 
FMV 
FMVC 
FM 
FA 
FM 
FMVC 
ABI 
B 

END 

(END OF FILE CNTRL) 


RETURN : DISPLACEMENT LISTING IN M2 SO 

CNTRL 

* 


112 ( Rl , 2 ) * , FR3 

5 6 ( Rl , 2 ) * , FR3 
116 { Rl , 2 ) * ,FR1 
60 (Rl , 2 ) * , FR1 
FR1 , FR3 

120( Rl , 2 ) * ,FR1 

6 4 ( Rl , 2 ) * , FR1 
FR1 , FR3 

124 ( Rl , 2 ) * , FRl 
68 (Rl , 2 ) * , FRl 
FR1 , FR3 

128 ( Rl , 2 ) * , FRl 
72 (Rl , 2 ) * ,FR1 
FRl ,FR3 

132 ( Rl , 2 ) * , FRl 
76 (Rl , 2 ) * , FRl 
FRl , FR3 
10 ( Rl , 0 ) * , FRl 
108 (Rl , 2 ) * ,FR1 
FRl , FR3 

FR3 , 136 ( Rl , 2 ) * 
12 ( Rl , 0 ) * , FRl 
108 (Rl , 2 ) * ,FR1 
FRl ,FR3 

104 ( Rl , 2 ) * , FR3 
FR3 , 8 ( Rl , 0 ) * 

4 ,R1 
RETURN 


FTHETA 

K1*FTHETA 

FMODE1 

K2*FMODl 

ADD TO CONTROL 

FMODE2 

K3*FMODE2 

ADD TO CONTROL 

FOMEGA 

K4*FOMEGA 

ADD TO CONTROL 

EST VEL MODE1 

K5*EST VEL MODEl 

ADD TO CONTROL 

EST VEL MODE 2 

K6*EST VEL MODE 2 

ADD TO CONTROL 

DESIRED ANGLE 

SCALE ANGLE 

ADD 

SAVE TORQ 
DITHER SIGNAL 
SCALE 

CONVERT TORQ 
SAVE INTEGER TORQ 


F . 5 . 5 Routine EST 

This routine is compiled using the utility routine $S1ASM. 




The program preparation facility accessed via $SMMAIN is the 
easiest method for its execution. The file ASMWK4 on volume 
CONTROL of the harddisk was reserved as the destination file 
for the compiled object. 


(START OF FILE EST) 


*********************************************************** 

* * * * * 




* * * * * 

LUENBERGER 

OBSERVOR FOR MODAL 

VELOCITIES 

* * * * * 




*********************************************************** 


START 




EXTRN 

RETURN : DISPLACEMENT LISTING IN 

* 


LUENEDX 


ENTRY 

EST 


EST 

EQU 

* 



FMV 

128 ( R1 , 2 ) * ,FR3 

START EQN1 GET VELMOD1 


FM 

0 ( R1 , 2 ) * , FR3 

Fll*VELMODl 


FMV 

132(R1,2)*,FR2 

GET* MODE 2 


FM 

4 ( Rl , 2 ) * , FR2 

F12*MODE2 


FA 

FR2 , FR3 

SUM TERMS FOR EQN 1 


FMV 

112(R1,2)*,FR2 

GET FTHETA 


FM 

8 ( Rl , 2 ) * , FR2 

G11*FTHETA 


FA 

FR2 , FR3 

SUM TERMS FOR EQN 1 


FMV 

116 (Rl , 2 ) * ,FR2 

GET MODI 


FM 

12 (Rl , 2 ) * , FR2 

G12*MODl 


FA 

FR2 , FR3 

SUM TERMS FOR EQN 1 


FMV 

120 ( Rl , 2 ) * , FR2 

GET MOD 2 


FM- 

16 (Rl , 2 ) * ,FR2 

G13*THETA 


FA 

FR2 , FR3 

SUM TERMS FOR EQN 1 


FMV 

1 2 4 ( Rl , 2 ) * , FR2 

GET FOMEGA 


FM 

2 0 ( Rl , 2 ) * , FR2 

G14*FOMEGA 


FA 

FR2 , FR3 

SUM TERMS FOR EQN 1 


FMV 

136(R1 , 2 ) * , FR2 

GET FTORQ 


FM 

2 4 ( Rl , 2 ) * , FR2 

Bl*TORQ 


FA 

FR2 , FR3 

COMPLETE EQN 1 


FMV 

128(R1 , 2 ) * ,FR2 

START EQN 2 GET VELMOD1 


FM 

28 (Rl , 2 ) * ,FR2 

F21*VELMODl 


FMV 

132(R1 , 2 ) * , FR1 

GET*MODE2 


FM 

3 2 ( Rl , 2 ) * , FR1 

F22*MODE2 


FA 

FR1 , FR2 

SUM TERMS FOR EQN 2 


FMV 

112 (Rl , 2 ) * , FRl 

GET FTHETA 


FM 

36 (Rl , 2 ) * , FR1 

G21*FTHETA 


FA 

FR1 , FR2 

SUM TERMS FOR EQN 2 


FMV 

116 (Rl , 2 ) * , FR1 

GET MODI 


FM 

40 (Rl , 2 ) * , FR1 

G22*M0D1 


FA 

FR1 , FR2 

SUM TERMS FOR EQN 2 


FMV 

120(R1 , 2 ) * ,FR1 

GET MOD 2 
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4 


44 ( R1 , 2 ) * ,FR1 :G23*THETA 

FR1 , FR2 : SUM TERMS FOR EQN 2 

124 ( R1 , 2 ) * , FR1 : GET FOMEGA 

48 (R1 , 2 ) * ,FR1 : G2 4* FOMEGA 

FR1 , FR2 : SUM TERMS FOR EQN 2 

136 ( Rl , 2 ) * , FR1 : GET FTORQ 

52{R1 , 2 ) * , FR1 :B2*TORQ 

FR1 , FR2 : COMPLETE EQN 2 

FR3 , 128 ( Rl , 2 ) * :SAVE EST VEL MODE1 

FR2 , 132 ( Rl , 2 ) * :SAVE EST VEL MODE 2 

4 Rl 

RETURN 
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